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

Формула изобретения

Система коррекции траектории движения манипулятора, содержащая задатчик программных сигналов, первый и второй выходы которого подключены соответственно к первым входам первого и второго сумматоров, отличающаяся тем, что в нее дополнительно введены последовательно соединенные первый косинусный функциональный преобразователь, первый и второй блоки умножения, и третий сумматор, выход которого подключен к первому выходу системы, последовательно соединенные второй синусный функциональный преобразователь, третий блок умножения, четвертый сумматор и четвертый блок умножения, последовательно соединенные третий синусный функциональный преобразователь, пятый и шестой блоки умножения, пятый сумматор и седьмой блок умножения, выход которого подключен ко второму входу третьего сумматора, а его третий вход - к выходу четвертого блока умножения, последовательно соединенные четвертый косинусный функциональный преобразователь, восьмой блок умножения, второй вход которого подключен к выходу третьего синусного функционального преобразователя и второму входу первого блока умножения, шестой сумматор, девятый блок умножения и седьмой сумматор, выход которого подключен ко второму выходу системы, последовательно соединенные пятый косинусный функциональный преобразователь, десятый и одиннадцатый блоки умножения, восьмой сумматор и двенадцатый блок умножения, последовательно соединенные шестой синусный функциональный преобразователь, тринадцатый блок умножения, девятый сумматор, четырнадцатый блок умножения и десятый сумматор, выход которого подключен к третьему выходу системы, последовательно соединенные пятнадцатый и шестнадцатый блоки умножения, причем выход последнего подключен ко второму входу седьмого сумматора, а его третий вход - к выходу двенадцатого блока умножения, последовательно соединенные семнадцатый блок умножения, первый вход которого подключен к выходу пятого косинусного функционального преобразователя и к первому входу пятнадцатого блока умножения, а второй - к выходу шестого синусного функционального преобразователя и второму входу пятого блока умножения, и восемнадцатый блок умножения, выход которого подключен ко второму входу шестого сумматора, а также блок гироскопов, первый выход которого подключен ко входам первого косинусного и второго синусного функциональных преобразователей, второй - ко входам четвертого косинусного и шестого синусного функциональных преобразователей, а третий - ко входам третьего синусного и пятого косинусного функциональных преобразователей, девятнадцатый блок умножения, первый вход которого подключен к выходу четвертого косинусного функционального преобразователя и второму входу десятого блока умножения, второй вход - к выходу второго сумматора и вторым входам четвертого и двенадцатого блоков умножения, а выход - ко второму входу девятого сумматора, двадцатый блок умножения, первый вход которого подключен к выходу первого сумматора и ко вторым входам второго и шестнадцатого блоков умножения, второй - к выходу второго синусного функционального преобразователя и ко вторым входам шестого, одиннадцатого и восемнадцатого блоков умножения, а выход - ко второму входу десятого сумматора, одиннадцатый сумматор, первый вход которого подключен к третьему выходу задатчика программных сигналов, второй вход - к первому выходу блока навигации, второй и третий выходы которого соединены соответственно со вторыми входами второго и первого сумматоров, а выход - ко вторым входам седьмого, девятого и тринадцатого блоков умножения, причем выход первого косинусного функционального преобразователя подключен ко вторым входам четырнадцатого и пятнадцатого блоков умножения, вторые входы четвертого, пятого и восьмого сумматоров подключены соответственно к выходам семнадцатого, десятого и пятого блоков умножения, а выход восьмого блока умножения - ко второму входу третьего блока умножения.

Описание изобретения к патенту

Изобретение относится к области автоматического управления динамическими объектами и может быть использовано при автоматическом управлении многозвенными манипуляторами, устанавливаемыми на подводных аппаратах.

Известно устройство для формирования траектории бортового наведения самолета, в котором блок скорости сближения подключен к последовательно соединенным делителю, инвертору, блоку угла упреждения и выходному буферу, а коммутатор подключен к блоку памяти, первый и второй выходы которого соединены соответственно с первым и вторым входами первого блока вычитания, выход которого соединен с первым входом блока смещения фиктивной цели, первый выход которого подключен к первому входу блока дальности и второму входу блока угла упреждения и к второму входу блока скорости сближения, а второй выход блока смещения фиктивной цели - к третьему входу блока скорости сближения и третьему входу блока угла упреждения, выход блока дальности соединен с четвертым входом блока скорости сближения и вторым входом делителя, пятый, шестой и седьмой входы блока скорости сближения подключены соответственно к пятому, шестому, седьмому выходам блока памяти, причем выход блока скорости сближения соединен с входом блока угла упреждения, а шестой и седьмой входы блока скорости сближения соединены соответственно с четвертым и пятым входом блока угла упреждения, кроме того, третий и четвертый выходы блока памяти соединены соответственно с первым и вторым входом второго блока вычитания, выход которого соединен с вторым входом блока смещения фиктивной цели, пятый выход блока памяти соединен с пятым входом блока скорости сближения, седьмой выход блока памяти соединен с шестым входом блока скорости сближения, восьмой выход блока памяти подключен к седьмому входу блока скорости сближения, пятый, девятый, десятый, одиннадцатый, двенадцатый, тринадцатый выходы блока памяти соединены соответственно с третьим, четвертым, пятым, шестым, седьмым и восьмым входами блока смещения фиктивной цели (см. патент РФ № 2136034. МПК G05D 1/02, 1999).

Недостатком указанного устройства является то, что оно предназначено для управления траекторией движения летательного аппарата, и его нельзя использовать для коррекции траектории движения рабочего органа манипулятора, установленного на подводном аппарате (ПА), поскольку для решения этой задачи необходимо реализовывать другие функции.

Известна также система управления траекторией движения объекта, содержащая объект управления, первый и второй сумматоры, задатчик программных сигналов, первый и второй выходы которого соединены соответственно с первыми входами первого и второго сумматоров, последовательно соединенные первый блок формирования ошибок, второй вход которого соединен с первым выходом объекта управления, первый корректирующий фильтр, последовательно соединенные второй блок формирования ошибок, второй вход которого соединен со вторым выходом объекта управления, второй корректирующий фильтр, на первые входы первого и второго блоков формирования ошибок поступает входной сигнал в вертикальном и горизонтальном каналах, в которую введены первый усилитель с регулируемым коэффициентом усиления, вход которого соединен с выходом первого корректирующего фильтра, второй усилитель с регулируемым коэффициентом усиления, вход которого соединен с выходом второго корректирующего фильтра, фазовый вращатель, первый и второй выходы которого соединены со вторыми входами соответственно первого и второго сумматоров, устройство вычисления коэффициента и фазы, первый и второй входы которого соединены с выходами соответственно первого и второго блоков формирования ошибок, причем первый и второй входы фазового вращателя соединены с выходами соответственно первого и второго усилителей с регулируемым коэффициентом усиления, управляющие входы которых соединены соответственно с первым и вторым выходами устройства вычисления коэффициента и фазы, первый и второй входы объекта управления подключены к выходам соответственно первого и второго сумматоров и соответственно к третьему и четвертому входам устройства вычисления коэффициента и фазы, а третий и четвертый выходы задатчика программных сигналов соединены соответственно с пятым и шестым входами устройства вычисления коэффициента и фазы, третий выход которого соединен с третьим входом фазового вращателя (см. патент РФ № 2206916, МПК G05D 1/08, G05B 23/02, 2003).

Данное устройство по своей сущности является наиболее близким к предлагаемому решению. Его недостатком является то, что оно также предназначено для формирования, а не коррекции уже сформированной траектории движения. При этом оно способно работать только при фазовой связи каналов и изменении коэффициента передачи объекта в условиях воздействия возмущающих факторов. В связи с отмеченным это устройство также нельзя использовать для решения поставленной задачи, связанной с коррекцией траектории движения рабочего органа манипулятора, установленного на ПА с целью обеспечения высокой точности выполнения рабочих операций с учетом текущих отклонений ПА от его исходного положения во время работы многозвенного манипулятора.

Задачей, на решение которой направлено заявляемое техническое решение, является обеспечение требуемой высокой точности движения рабочего органа манипулятора по желаемой пространственной траектории с учетом неизбежных текущих отклонений ПА, вызванных возмущающими воздействиями со стороны работающего манипулятора, кабель-тросса и внешней среды, от его исходного положения, для которого и рассчитывается эта траектория.

Технический результат, который может быть получен при реализации заявляемого технического решения, выражается в автоматической коррекции исходной программной траектории движения рабочего органа манипулятора с учетом текущего углового и линейного смещения ПА, на котором он установлен, от его исходного положения. Это позволяет сохранить высокую точность перемещения рабочего органа по желаемой траектории.

Поставленная задача решается тем, что в систему коррекции траектории движения манипулятора, содержащую задатчик программных сигналов, первый и второй выходы которого подключены соответственно к первым входам первого и второго сумматоров, дополнительно вводятся последовательно соединенные первый косинусный функциональный преобразователь, первый и второй блоки умножения и третий сумматор, выход которого подключен к первому выходу системы, последовательно соединенные второй синусный функциональный преобразователь, третий блок умножения, четвертый сумматор и четвертый блок умножения, последовательно соединенные третий синусный функциональный преобразователь, пятый и шестой блоки умножения, пятый сумматор и седьмой блок умножения, выход которого подключен ко второму входу третьего сумматора, а его третий вход - к выходу четвертого блока умножения, последовательно соединенные четвертый косинусный функциональный преобразователь, восьмой блок умножения, второй вход которого подключен к выходу третьего синусного функционального преобразователя и второму входу первого блока умножения, шестой сумматор, девятый блок умножения и седьмой сумматор, выход которого подключен ко второму выходу системы, последовательно соединенные пятый косинусный функциональный преобразователь, десятый и одиннадцатый блоки умножения, восьмой сумматор и двенадцатый блок умножения, последовательно соединенные шестой синусный функциональный преобразователь, тринадцатый блок умножения, девятый сумматор, четырнадцатый блок умножения и десятый сумматор, выход которого подключен к третьему выходу системы, последовательно соединенные пятнадцатый и шестнадцатый блоки умножения, причем выход последнего подключен ко второму входу седьмого сумматора, а его третий вход - к выходу двенадцатого блока умножения, последовательно соединенные семнадцатый блок умножения, первый вход которого подключен к выходу пятого косинусного функционального преобразователя и к первому входу пятнадцатого блока умножения, а второй - к выходу шестого синусного функционального преобразователя и второму входу пятого блока умножения, и восемнадцатый блок умножения, выход которого подключен ко второму входу шестого сумматора, а также блок гироскопов, первый выход которого подключен ко входам первого косинусного и второго синусного функциональных преобразователей, второй - ко входам четвертого косинусного и шестого синусного функциональных преобразователей, а третий - ко входам третьего синусного и пятого косинусного функциональных преобразователей, девятнадцатый блок умножения, первый вход которого подключен к выходу четвертого косинусного функционального преобразователя и второму входу десятого блока умножения, второй вход - к выходу второго сумматора и вторым входам четвертого и двенадцатого блоков умножения, а выход - ко второму входу девятого сумматора, двадцатый блок умножения, первый вход которого подключен к выходу первого сумматора и ко вторым входам второго и шестнадцатого блоков умножения, второй - к выходу второго синусного функционального преобразователя и ко вторым входам шестого, одиннадцатого и восемнадцатого блоков умножения, а выход - ко второму входу десятого сумматора, одиннадцатый сумматор, первый вход которого подключен к третьему выходу задатчика программных сигналов, второй вход - к первому выходу блока навигации, второй и третий выходы которого соединены соответственно со вторыми входами второго и первого сумматоров, а выход - ко вторым входам седьмого, девятого и тринадцатого блоков умножения, причем выход первого косинусного функционального преобразователя подключен ко вторым входам четырнадцатого и пятнадцатого блоков умножения, вторые входы четвертого, пятого и восьмого сумматоров подключены соответственно к выходам семнадцатого, десятого и пятого блоков умножения, а выход восьмого блока умножения - ко второму входу третьего блока умножения.

Сопоставительный анализ заявляемого технического решения с его аналогом и прототипом свидетельствует о его соответствии критерию "новизна".

Заявленная совокупность признаков, приведенная в отличительной части формулы изобретения, позволяет добиться высокой точности движения рабочего органа манипулятора по желаемой траектории с учетом неизбежных текущих отклонений ПА, вызванных возмущающими воздействиями со стороны работающего манипулятора, кабель-тросса и внешней среды, от его исходного положения, для которого и рассчитывается желаемая пространственная траектория движения рабочего органа манипулятора.

Блок-схема предлагаемой системы коррекции траектории движения рабочего органа манипулятора, установленного на ПА, представлена на фиг.1. На фиг.2 схематически показано исходное положение ПА и исходное положение установленного на нем манипулятора, а также смещенное положение ПА и новое положение манипулятора, учитывающее текущее смещение ПА относительно исходного, при котором неизменно происходит точное перемещение рабочего органа манипулятора по желаемой пространственной траектории; на фиг.3 изображены угловые перемещения связанной с ПА системы координат (СК).

Система коррекции траектории движения манипулятора содержит задатчик 1 программных сигналов, первый 2 и второй 3 сумматоры, первый косинусный функциональный преобразователь 4, первый 5 и второй 6 блоки умножения, третий сумматор 7, второй синусный функциональный преобразователь 8, третий блок 9 умножения, четвертый сумматор 10, четвертый блок 11 умножения, третий синусный функциональный преобразователь 12, пятый 13 и шестой 14 блоки умножения, пятый сумматор 15, седьмой блок 16 умножения, четвертый косинусный функциональный преобразователь 17, восьмой блок 18 умножения, шестой сумматор 19, девятый блок 20 умножения, седьмой сумматор 21, пятый косинусный функциональный преобразователь 22, десятый 23 и одиннадцатый 24 блоки умножения, восьмой сумматор 25, двенадцатый блок 26 умножения, шестой синусный функциональный преобразователь 27, тринадцатый блок 28 умножения, девятый сумматор 29, четырнадцатый блок 30 умножения, десятый сумматор 31, пятнадцатый 32, шестнадцатый 33, семнадцатый 34 и восемнадцатый 35 блоки умножения, блок 36 гироскопов, девятнадцатый 37 и двадцатый 38 блоки умножения, одиннадцатый сумматор 39 и блок 40 навигации.

На фиг.1, 2 и 3 введены следующие обозначения: I - заданная (неизменная) желаемая траектория пространственного движения рабочего органа n - степенного манипулятора; II - ПА в исходном положении; III - манипулятор в исходном положении; IV - ПА в положении, смещенном относительно исходного; V - новое положение манипулятора, обеспечивающее точное прохождение рабочего инструмента по траектории I; С - центр масс ПА; А - текущая характерная точка рабочего органа манипулятора на траектории I; О - точка крепления манипулятора к ПА; XYZ и Х^*Y^* Z^* - соответственно, абсолютная и жестко связанная с корпусом ПА СК, оси которых в исходном положении ПА II совпадают, а их начала совмещены с точкой С. При этом ось Y^* является продольной осью ПА; Y^*' и Х^* ' - соответственно, проекции осей X^* и Y^* в горизонтальной плоскости; Z^*' - проекция оси Z^* на вертикальную плоскость; P^*(t) R³ - вектор, задающий текущее желаемое положение точки А в СК Х^*Y^*Z^*, в которой работает манипулятор; P(t)^T R³ - вектор, задающий положение точки А в CK XYZ; х, у, z - координаты точки А в СК XYZ; х^*, у ^*, z^* - координаты точки А в СК X^* Y^*Z^*; , , - соответственно, текущие угловые отклонения ПА по тангажу, крену и курсу от его исходного положения ПА (они измеряются блоком 36); x, у, z - смещения точки С от ее исходного положения по соответствующим осям являются координатами вектора P_C(t) R³ в СК XYZ (они формируются с помощью задатчика 1 и блока 40).

До начала работы манипулятора, установленного на ПА, в режиме его зависания над или вблизи объекта работ определяется и задается желаемая траектория движения рабочего органа манипулятора, которая должна быть точно отработана последним после вывода ПА в заданную точку пространства. Однако во время работы в режиме зависания ПА незапланированно смещается из его исходного положения под воздействием течений, волнения моря, а также ввиду силовых и моментных воздействий со стороны кабель-троса и работающего манипулятора даже при наличии системы его автоматической стабилизации [Филаретов В.Ф. и др. Системы управления подводными роботами. - М.: Круглый год, 2001, с.171-179; 223-227], которая имеет ограниченную точность и инерционность. Это затрудняет автоматическое выполнение многих манипуляционных операций. В результате возникает задача использования дополнительной автоматической коррекции программной траектории движения рабочего органа манипулятора в связанной с ним СК уже в процессе его движения. Эта коррекция должна осуществляться на основе информации о реальном угловом и линейном смещении ПА относительно его исходного положения, получаемой от навигационных и гироскопических устройств, а также информации о текущей конфигурации манипулятора, обеспечивая дополнительное перемещение его рабочего органа. При этом угловые и линейные смещения ПА относительно его исходного положения при наличии указанной выше системы автоматической стабилизации не должны превышать некоторых пределов, зависящих от параметров и кинематической схемы манипулятора. То есть конструкция манипулятора в процессе отработки перемещающейся по траектории I точки А должна быть способна отработать возникающие смещения ПА из исходного положения.

Система работает следующим образом. На первом, втором и третьем выходах задатчика 1 программных сигналов формируются сигналы: z, у и х соответственно, а на первом, втором и третьем выходах блока 40 навигации - сигналы: х, у и z соответственно. Первые положительные (со стороны задатчика 1) и вторые отрицательные входы сумматоров 2, 3 и 39 имеют единичные коэффициенты усиления. В результате на выходах сумматоров 2, 3 и 39 формируются сигналы: (z- z), (у- у) и (х- х) соответственно.

На первом, втором и третьем выходах блока 36 гироскопов формируются сигналы , и соответственно. На выходах блоков 5, 13, 18, 23, 32, 34 и 9 формируются сигналы: S C , S S , S C , C C , C C , S C и S S C соответственно, на выходе блока 14 - сигнал S S S , а на выходе блока 6 - сигнал S C (z- z), где S =sin ; S =sin ; С =cos , C =cos , С =cos . Поскольку положительные входы сумматора 10 имеют единичные коэффициенты усиления, то на выходе блока 11 формируется сигнал (S C +S S C )(у- у). Поскольку первый отрицательный (со стороны блока 14) и второй положительный входы сумматора 15 имеют единичные коэффициенты усиления, то на выходе блока 16 формируется сигнал (С С -S S S )(x- х). Первый отрицательный (со стороны блока 6), а также второй и третий положительные входы сумматора 7 имеют единичные коэффициенты усиления, в результате на его выходе формируется сигнал х^*=(С С -S S S )(x- х)+(S C +S S C )(у- у)-S C (z- z).

На выходе блоков 28, 37 и 38 формируются сигналы S (x- x), С (у- у) и S (z- z) соответственно. Первый отрицательный (со стороны блока 28) и второй положительный входы сумматора 29 имеют единичные коэффициенты усиления, поэтому на выходе блока 30 формируется сигнал C (C (у- у)-S (x- x)). Положительные входы сумматора 31 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал ^*=С (С (у- у)-S (x- х))+S (z- z).

На выходе блоков 24, 33 и 35 формируются сигналы S C C , C C (z- z) и S S C соответственно. Поскольку первый отрицательный (со стороны блока 24) и второй положительный входы сумматора 25, а также все положительные входы сумматора 19 имеют единичные коэффициенты усиления, то на выходе блока 26 формируется сигнал (S S -S C C )(у- у), а на выходе блока 20 - сигнал (S C +S S C )(x- x). Все положительные ходы сумматора 21 имеют единичные коэффициенты усиления, в результате на его выходе формируется сигнал

z^*=(S C +S S C )(х- x)+(S S -S C C )(у- у)+C C (z- z).

Очевидно, что элементы вектора P*(t) в СК XYZ можно получить из разности P(t)-P_C(t), а в СК X^*Y^*Z^* - с помощью выражения

где R R^3×3 - матрица поворотов СК X^* Y^*Z^* относительно СК XYZ, Т - символ транспонирования.

Для составления матрицы R необходимо представить поворот ПА вместе с СК X^*Y^*Z^* в виде последовательности элементарных поворотов. Оси, относительно которых отсчитываются углы соответствующих поворотов СК X ^*Y^*Z^*, и последовательность этих поворотов должны выбираться такими, чтобы получаемые при этом углы , и действительно соответствовали углам, которые будут измеряться гироскопами блока 36. Это условие выполняется при следующей последовательности элементарных поворотов СК X^*Y^*Z^* : вначале осуществляется поворот на угол вокруг оси Z (ему соответствует матрица элементарного поворота R_Z, ), затем - поворот на угол вокруг повернутой оси Х^*' (ему соответствует матрица элементарного поворота R_X*', ), и, наконец, поворот на угол вокруг оси Y^* (ему соответствует матрица элементарного поворота R_Y*, ). Описанная последовательность поворотов показана на фиг.3. Указанные матрицы поворотов имеют стандартный вид

Учитывая выражения (2), несложно получить матрицу R для описанной последовательности поворотов СК X ^*Y^*Z^* вместе с ПА

После подстановки транспонированной матрицы R (3) в выражение (1) будем иметь

Очевидно, что все три элемента вектора P^*(t) R³ формируются на соответствующих выходах предложенной системы (см. фиг.1). Этот вектор позволяет так управлять рабочим органом манипулятора, чтобы он продолжал двигаться по желаемой траектории при существовании неизбежных текущих отклонений ПА от его исходного положения, для которого и рассчитывается желаемая пространственная траектория движения этого рабочего органа.