RU2462745C1 - System for adjusting motion path of manipulator - Google Patents
System for adjusting motion path of manipulator Download PDFInfo
- Publication number
- RU2462745C1 RU2462745C1 RU2011135581/08A RU2011135581A RU2462745C1 RU 2462745 C1 RU2462745 C1 RU 2462745C1 RU 2011135581/08 A RU2011135581/08 A RU 2011135581/08A RU 2011135581 A RU2011135581 A RU 2011135581A RU 2462745 C1 RU2462745 C1 RU 2462745C1
- Authority
- RU
- Russia
- Prior art keywords
- output
- multiplication
- input
- adder
- inputs
- Prior art date
Links
Images
Landscapes
- Numerical Control (AREA)
Abstract
Description
Изобретение относится к области автоматического управления динамическими объектами и может быть использовано при автоматическом управлении многозвенными манипуляторами, устанавливаемыми на подводных аппаратах.The invention relates to the field of automatic control of dynamic objects and can be used for automatic control of multi-link manipulators mounted on underwater vehicles.
Известно устройство для формирования траектории бортового наведения самолета, в котором блок скорости сближения подключен к последовательно соединенным делителю, инвертору, блоку угла упреждения и выходному буферу, а коммутатор подключен к блоку памяти, первый и второй выходы которого соединены соответственно с первым и вторым входами первого блока вычитания, выход которого соединен с первым входом блока смещения фиктивной цели, первый выход которого подключен к первому входу блока дальности и второму входу блока угла упреждения и к второму входу блока скорости сближения, а второй выход блока смещения фиктивной цели - к третьему входу блока скорости сближения и третьему входу блока угла упреждения, выход блока дальности соединен с четвертым входом блока скорости сближения и вторым входом делителя, пятый, шестой и седьмой входы блока скорости сближения подключены соответственно к пятому, шестому, седьмому выходам блока памяти, причем выход блока скорости сближения соединен с входом блока угла упреждения, а шестой и седьмой входы блока скорости сближения соединены соответственно с четвертым и пятым входом блока угла упреждения, кроме того, третий и четвертый выходы блока памяти соединены соответственно с первым и вторым входом второго блока вычитания, выход которого соединен с вторым входом блока смещения фиктивной цели, пятый выход блока памяти соединен с пятым входом блока скорости сближения, седьмой выход блока памяти соединен с шестым входом блока скорости сближения, восьмой выход блока памяти подключен к седьмому входу блока скорости сближения, пятый, девятый, десятый, одиннадцатый, двенадцатый, тринадцатый выходы блока памяти соединены соответственно с третьим, четвертым, пятым, шестым, седьмым и восьмым входами блока смещения фиктивной цели (см. патент РФ №2136034. МПК G05D 1/02, 1999).A device is known for forming an aircraft guidance path in which an approaching speed unit is connected to a divider, an inverter, a lead angle unit and an output buffer connected in series, and the switch is connected to a memory unit, the first and second outputs of which are connected respectively to the first and second inputs of the first block subtraction, the output of which is connected to the first input of the bias block of the fictitious target, the first output of which is connected to the first input of the range block and the second input of the lead block and to the second input of the convergence speed unit, and the second output of the fictitious target displacement unit - to the third input of the approach speed unit and the third input of the lead unit, the output of the range unit is connected to the fourth input of the approach speed unit and the second input of the divider, the fifth, sixth and seventh inputs of the speed unit the proximity are connected respectively to the fifth, sixth, and seventh outputs of the memory unit, the output of the proximity unit is connected to the input of the lead angle unit, and the sixth and seventh inputs of the proximity unit are connected respectively, with the fourth and fifth input of the lead block, in addition, the third and fourth outputs of the memory block are connected respectively to the first and second input of the second subtraction block, the output of which is connected to the second input of the bias block of the dummy target, the fifth output of the memory block is connected to the fifth input of the block approach speed, the seventh output of the memory unit is connected to the sixth input of the approach speed unit, the eighth output of the memory unit is connected to the seventh input of the approach speed unit, fifth, ninth, tenth, eleventh, twelfth, the thirteenth outputs of the memory block are connected, respectively, to the third, fourth, fifth, sixth, seventh and eighth inputs of the bias unit of the fictitious target (see RF patent No. 2136034. IPC G05D 1/02, 1999).
Недостатком указанного устройства является то, что оно предназначено для управления траекторией движения летательного аппарата, и его нельзя использовать для коррекции траектории движения рабочего органа манипулятора, установленного на подводном аппарате (ПА), поскольку для решения этой задачи необходимо реализовывать другие функции.The disadvantage of this device is that it is designed to control the trajectory of the aircraft, and it can not be used to correct the trajectory of the working body of the manipulator mounted on the underwater vehicle (PA), because to solve this problem it is necessary to implement other functions.
Известна также система управления траекторией движения объекта, содержащая объект управления, первый и второй сумматоры, задатчик программных сигналов, первый и второй выходы которого соединены соответственно с первыми входами первого и второго сумматоров, последовательно соединенные первый блок формирования ошибок, второй вход которого соединен с первым выходом объекта управления, первый корректирующий фильтр, последовательно соединенные второй блок формирования ошибок, второй вход которого соединен со вторым выходом объекта управления, второй корректирующий фильтр, на первые входы первого и второго блоков формирования ошибок поступает входной сигнал в вертикальном и горизонтальном каналах, в которую введены первый усилитель с регулируемым коэффициентом усиления, вход которого соединен с выходом первого корректирующего фильтра, второй усилитель с регулируемым коэффициентом усиления, вход которого соединен с выходом второго корректирующего фильтра, фазовый вращатель, первый и второй выходы которого соединены со вторыми входами соответственно первого и второго сумматоров, устройство вычисления коэффициента и фазы, первый и второй входы которого соединены с выходами соответственно первого и второго блоков формирования ошибок, причем первый и второй входы фазового вращателя соединены с выходами соответственно первого и второго усилителей с регулируемым коэффициентом усиления, управляющие входы которых соединены соответственно с первым и вторым выходами устройства вычисления коэффициента и фазы, первый и второй входы объекта управления подключены к выходам соответственно первого и второго сумматоров и соответственно к третьему и четвертому входам устройства вычисления коэффициента и фазы, а третий и четвертый выходы задатчика программных сигналов соединены соответственно с пятым и шестым входами устройства вычисления коэффициента и фазы, третий выход которого соединен с третьим входом фазового вращателя (см. патент РФ №2206916, МПК G05D 1/08, G05B 23/02, 2003).Also known is a system for controlling the object’s trajectory, comprising a control object, first and second adders, a program signal generator, the first and second outputs of which are connected respectively to the first inputs of the first and second adders, the first error generating unit connected in series, the second input of which is connected to the first output control object, the first correction filter, connected in series to the second error generation unit, the second input of which is connected to the second output of the control unit phenomena, the second correction filter, the input signal in the vertical and horizontal channels, into which the first amplifier with adjustable gain is input, the input of which is connected to the output of the first correction filter, the second amplifier with adjustable gain, is fed to the first inputs of the first and second error generation units, the input of which is connected to the output of the second correction filter, a phase rotator, the first and second outputs of which are connected to the second inputs of the first and second, respectively of adders, a coefficient and phase calculator, the first and second inputs of which are connected to the outputs of the first and second error generating units, respectively, the first and second inputs of the phase rotator connected to the outputs of the first and second amplifiers with adjustable gain, the control inputs of which are connected respectively with the first and second outputs of the device for calculating the coefficient and phase, the first and second inputs of the control object are connected to the outputs of the first and second and adders for respectively to third and fourth inputs of the computing device and the phase coefficient, and the third and fourth outputs of the setpoint programming signals coupled respectively to the fifth and sixth inputs factor calculating device and the phases, the third output of which is connected to the third input of the phase rotator (cm. RF patent No. 2206916, IPC
Данное устройство по своей сущности является наиболее близким к предлагаемому решению. Его недостатком является то, что оно также предназначено для формирования, а не коррекции уже сформированной траектории движения. При этом оно способно работать только при фазовой связи каналов и изменении коэффициента передачи объекта в условиях воздействия возмущающих факторов. В связи с отмеченным это устройство также нельзя использовать для решения поставленной задачи, связанной с коррекцией траектории движения рабочего органа манипулятора, установленного на ПА с целью обеспечения высокой точности выполнения рабочих операций с учетом текущих отклонений ПА от его исходного положения во время работы многозвенного манипулятора.This device in essence is the closest to the proposed solution. Its disadvantage is that it is also intended to form, rather than correct, an already formed trajectory of movement. Moreover, it is able to work only with phase coupling of channels and a change in the transmission coefficient of an object under the influence of disturbing factors. In connection with the aforementioned, this device can also not be used to solve the problem associated with the correction of the trajectory of the working body of the manipulator mounted on the PA in order to ensure high accuracy of work operations taking into account the current deviations of the PA from its initial position during operation of the multi-link manipulator.
Задачей, на решение которой направлено заявляемое техническое решение, является обеспечение требуемой высокой точности движения рабочего органа манипулятора по желаемой пространственной траектории с учетом неизбежных текущих отклонений ПА, вызванных возмущающими воздействиями со стороны работающего манипулятора, кабель-тросса и внешней среды, от его исходного положения, для которого и рассчитывается эта траектория.The task to which the claimed technical solution is directed is to provide the required high accuracy of the movement of the manipulator's working body along the desired spatial trajectory, taking into account the inevitable current deviations of the PA caused by disturbing influences from the working manipulator, cable cable and the external environment from its initial position, for which this trajectory is calculated.
Технический результат, который может быть получен при реализации заявляемого технического решения, выражается в автоматической коррекции исходной программной траектории движения рабочего органа манипулятора с учетом текущего углового и линейного смещения ПА, на котором он установлен, от его исходного положения. Это позволяет сохранить высокую точность перемещения рабочего органа по желаемой траектории.The technical result that can be obtained by implementing the proposed technical solution is expressed in automatic correction of the initial programmed path of the working body of the manipulator, taking into account the current angular and linear displacement of the PA on which it is installed, from its initial position. This allows you to maintain high accuracy of movement of the working body along the desired path.
Поставленная задача решается тем, что в систему коррекции траектории движения манипулятора, содержащую задатчик программных сигналов, первый и второй выходы которого подключены соответственно к первым входам первого и второго сумматоров, дополнительно вводятся последовательно соединенные первый косинусный функциональный преобразователь, первый и второй блоки умножения и третий сумматор, выход которого подключен к первому выходу системы, последовательно соединенные второй синусный функциональный преобразователь, третий блок умножения, четвертый сумматор и четвертый блок умножения, последовательно соединенные третий синусный функциональный преобразователь, пятый и шестой блоки умножения, пятый сумматор и седьмой блок умножения, выход которого подключен ко второму входу третьего сумматора, а его третий вход - к выходу четвертого блока умножения, последовательно соединенные четвертый косинусный функциональный преобразователь, восьмой блок умножения, второй вход которого подключен к выходу третьего синусного функционального преобразователя и второму входу первого блока умножения, шестой сумматор, девятый блок умножения и седьмой сумматор, выход которого подключен ко второму выходу системы, последовательно соединенные пятый косинусный функциональный преобразователь, десятый и одиннадцатый блоки умножения, восьмой сумматор и двенадцатый блок умножения, последовательно соединенные шестой синусный функциональный преобразователь, тринадцатый блок умножения, девятый сумматор, четырнадцатый блок умножения и десятый сумматор, выход которого подключен к третьему выходу системы, последовательно соединенные пятнадцатый и шестнадцатый блоки умножения, причем выход последнего подключен ко второму входу седьмого сумматора, а его третий вход - к выходу двенадцатого блока умножения, последовательно соединенные семнадцатый блок умножения, первый вход которого подключен к выходу пятого косинусного функционального преобразователя и к первому входу пятнадцатого блока умножения, а второй - к выходу шестого синусного функционального преобразователя и второму входу пятого блока умножения, и восемнадцатый блок умножения, выход которого подключен ко второму входу шестого сумматора, а также блок гироскопов, первый выход которого подключен ко входам первого косинусного и второго синусного функциональных преобразователей, второй - ко входам четвертого косинусного и шестого синусного функциональных преобразователей, а третий - ко входам третьего синусного и пятого косинусного функциональных преобразователей, девятнадцатый блок умножения, первый вход которого подключен к выходу четвертого косинусного функционального преобразователя и второму входу десятого блока умножения, второй вход - к выходу второго сумматора и вторым входам четвертого и двенадцатого блоков умножения, а выход - ко второму входу девятого сумматора, двадцатый блок умножения, первый вход которого подключен к выходу первого сумматора и ко вторым входам второго и шестнадцатого блоков умножения, второй - к выходу второго синусного функционального преобразователя и ко вторым входам шестого, одиннадцатого и восемнадцатого блоков умножения, а выход - ко второму входу десятого сумматора, одиннадцатый сумматор, первый вход которого подключен к третьему выходу задатчика программных сигналов, второй вход - к первому выходу блока навигации, второй и третий выходы которого соединены соответственно со вторыми входами второго и первого сумматоров, а выход - ко вторым входам седьмого, девятого и тринадцатого блоков умножения, причем выход первого косинусного функционального преобразователя подключен ко вторым входам четырнадцатого и пятнадцатого блоков умножения, вторые входы четвертого, пятого и восьмого сумматоров подключены соответственно к выходам семнадцатого, десятого и пятого блоков умножения, а выход восьмого блока умножения - ко второму входу третьего блока умножения.The problem is solved in that in the system of correction of the trajectory of the manipulator, containing the program signal generator, the first and second outputs of which are connected respectively to the first inputs of the first and second adders, additionally connected are the first cosine functional converter, the first and second multiplication units and the third adder the output of which is connected to the first output of the system, the second sine function converter is connected in series, the third block is multiplied the fourth adder and the fourth multiplication unit, connected in series with the third sine functional converter, the fifth and sixth multiplication units, the fifth adder and the seventh multiplication unit, the output of which is connected to the second input of the third adder, and its third input - to the output of the fourth multiplication unit, in series connected the fourth cosine functional converter, the eighth multiplication unit, the second input of which is connected to the output of the third sine functional converter and the second input of the first th multiplication block, sixth adder, ninth multiplication block and seventh adder, the output of which is connected to the second output of the system, the fifth cosine functional converter connected in series, the tenth and eleventh multiplication blocks, the eighth adder and the twelfth multiplication block, the sixth sine functional converter connected in series, thirteenth multiplication block, ninth adder, fourteenth multiplication block and tenth adder, the output of which is connected to the third output of the system, in series connected the fifteenth and sixteenth multiplication units, the output of the latter being connected to the second input of the seventh adder, and its third input to the output of the twelfth multiplication unit, the seventeenth multiplication unit connected in series, the first input of which is connected to the output of the fifth cosine functional converter and to the first input of the fifteenth block multiplication, and the second to the output of the sixth sine functional converter and the second input of the fifth block of multiplication, and the eighteenth block of multiplication, the output of which connected to the second input of the sixth adder, as well as a gyroscope unit, the first output of which is connected to the inputs of the first cosine and second sine functional converters, the second to the inputs of the fourth cosine and sixth sine functional converters, and the third to the inputs of the third sine and fifth cosine functional converters , the nineteenth multiplication block, the first input of which is connected to the output of the fourth cosine functional converter and the second input of the tenth block the second input is to the output of the second adder and the second inputs of the fourth and twelfth multiplication blocks, and the output is to the second input of the ninth adder, the twentieth multiplication block, the first input of which is connected to the output of the first adder and to the second inputs of the second and sixteenth multiplication blocks, the second - to the output of the second sine functional converter and to the second inputs of the sixth, eleventh and eighteenth multiplication blocks, and the output to the second input of the tenth adder, the eleventh adder, the first input of which is connected to the third output of the software signal setter, the second input to the first output of the navigation unit, the second and third outputs of which are connected respectively to the second inputs of the second and first adders, and the output to the second inputs of the seventh, ninth and thirteenth multiplication blocks, and the output of the first cosine functional the converter is connected to the second inputs of the fourteenth and fifteenth multiplication blocks, the second inputs of the fourth, fifth and eighth adders are connected respectively to the outputs of the seventeenth, tenth and p of multiplication units, and the output of the eighth multiplier - to the second input of the third multiplier.
Сопоставительный анализ заявляемого технического решения с его аналогом и прототипом свидетельствует о его соответствии критерию "новизна".A comparative analysis of the proposed technical solution with its analogue and prototype indicates its compliance with the criterion of "novelty."
Заявленная совокупность признаков, приведенная в отличительной части формулы изобретения, позволяет добиться высокой точности движения рабочего органа манипулятора по желаемой траектории с учетом неизбежных текущих отклонений ПА, вызванных возмущающими воздействиями со стороны работающего манипулятора, кабель-тросса и внешней среды, от его исходного положения, для которого и рассчитывается желаемая пространственная траектория движения рабочего органа манипулятора.The claimed combination of features, given in the characterizing part of the claims, allows to achieve high accuracy of movement of the working body of the manipulator along the desired path, taking into account the inevitable current deviations of the PA caused by disturbing influences from the working manipulator, cable cable and the external environment from its initial position, for which is calculated the desired spatial trajectory of the working body of the manipulator.
Блок-схема предлагаемой системы коррекции траектории движения рабочего органа манипулятора, установленного на ПА, представлена на фиг.1. На фиг.2 схематически показано исходное положение ПА и исходное положение установленного на нем манипулятора, а также смещенное положение ПА и новое положение манипулятора, учитывающее текущее смещение ПА относительно исходного, при котором неизменно происходит точное перемещение рабочего органа манипулятора по желаемой пространственной траектории; на фиг.3 изображены угловые перемещения связанной с ПА системы координат (СК).The block diagram of the proposed system for the correction of the trajectory of the working body of the manipulator mounted on the PA is presented in figure 1. Figure 2 schematically shows the initial position of the PA and the initial position of the manipulator installed on it, as well as the displaced position of the PA and the new position of the manipulator, taking into account the current offset of the PA relative to the initial one, at which the precise movement of the manipulator's working body along the desired spatial path occurs; figure 3 shows the angular displacement associated with the PA coordinate system (SC).
Система коррекции траектории движения манипулятора содержит задатчик 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 навигации.The manipulator trajectory correction system comprises a
На фиг.1, 2 и 3 введены следующие обозначения: I - заданная (неизменная) желаемая траектория пространственного движения рабочего органа n - степенного манипулятора; II - ПА в исходном положении; III - манипулятор в исходном положении; IV - ПА в положении, смещенном относительно исходного; V - новое положение манипулятора, обеспечивающее точное прохождение рабочего инструмента по траектории I; С - центр масс ПА; А - текущая характерная точка рабочего органа манипулятора на траектории I; О - точка крепления манипулятора к ПА; XYZ и Х*Y*Z* - соответственно, абсолютная и жестко связанная с корпусом ПА СК, оси которых в исходном положении ПА II совпадают, а их начала совмещены с точкой С. При этом ось Y* является продольной осью ПА; Y*' и Х*' - соответственно, проекции осей X* и Y* в горизонтальной плоскости; Z*' - проекция оси Z* на вертикальную плоскость; P*(t)∈R3 - вектор, задающий текущее желаемое положение точки А в СК Х*Y*Z*, в которой работает манипулятор; P(t)T∈R3 - вектор, задающий положение точки А в CK XYZ; х, у, z - координаты точки А в СК XYZ; х*, у*, z* - координаты точки А в СК X*Y*Z*; α, γ, β - соответственно, текущие угловые отклонения ПА по тангажу, крену и курсу от его исходного положения ПА (они измеряются блоком 36); Δx, Δу, Δz - смещения точки С от ее исходного положения по соответствующим осям являются координатами вектора PC(t)∈R3 в СК XYZ (они формируются с помощью задатчика 1 и блока 40).In figures 1, 2 and 3, the following notation is introduced: I - a given (unchanged) desired trajectory of the spatial movement of the working body of the n - power manipulator; II - PA in the initial position; III - the manipulator in the initial position; IV - PA in a position offset from the original; V - a new position of the manipulator, providing accurate passage of the working tool along the path I; C is the center of mass of PA; A is the current characteristic point of the working body of the manipulator on the trajectory I; O is the attachment point of the manipulator to the PA; XYZ and X * Y * Z * are, respectively, absolute and rigidly connected with the body of the PA SC, the axes of which in the initial position of PA II coincide, and their beginnings are aligned with point C. Moreover, the Y * axis is the longitudinal axis of the PA; Y * 'and X * ' - respectively, the projections of the axes X * and Y * in the horizontal plane; Z * '- the projection of the Z * axis on the vertical plane; P * (t) ∈R 3 - a vector that specifies the current desired position of point A in SC X * Y * Z * , in which the manipulator operates; P (t) T ∈R 3 is a vector defining the position of point A in CK XYZ; x, y, z - coordinates of point A in SK XYZ; x * , y * , z * are the coordinates of point A in the SC X * Y * Z * ; α, γ, β - respectively, the current angular deviations of the PA in pitch, roll and heading from its initial position of the PA (they are measured by block 36); Δx, Δy, Δz - the displacements of point C from its initial position along the corresponding axes are the coordinates of the vector P C (t) ∈R 3 in SC XYZ (they are formed using the
До начала работы манипулятора, установленного на ПА, в режиме его зависания над или вблизи объекта работ определяется и задается желаемая траектория движения рабочего органа манипулятора, которая должна быть точно отработана последним после вывода ПА в заданную точку пространства. Однако во время работы в режиме зависания ПА незапланированно смещается из его исходного положения под воздействием течений, волнения моря, а также ввиду силовых и моментных воздействий со стороны кабель-троса и работающего манипулятора даже при наличии системы его автоматической стабилизации [Филаретов В.Ф. и др. Системы управления подводными роботами. - М.: Круглый год, 2001, с.171-179; 223-227], которая имеет ограниченную точность и инерционность. Это затрудняет автоматическое выполнение многих манипуляционных операций. В результате возникает задача использования дополнительной автоматической коррекции программной траектории движения рабочего органа манипулятора в связанной с ним СК уже в процессе его движения. Эта коррекция должна осуществляться на основе информации о реальном угловом и линейном смещении ПА относительно его исходного положения, получаемой от навигационных и гироскопических устройств, а также информации о текущей конфигурации манипулятора, обеспечивая дополнительное перемещение его рабочего органа. При этом угловые и линейные смещения ПА относительно его исходного положения при наличии указанной выше системы автоматической стабилизации не должны превышать некоторых пределов, зависящих от параметров и кинематической схемы манипулятора. То есть конструкция манипулятора в процессе отработки перемещающейся по траектории I точки А должна быть способна отработать возникающие смещения ПА из исходного положения.Before the operation of the manipulator mounted on the PA, in the mode of its hovering above or near the work object, the desired trajectory of the manipulator's working body is determined and set, which must be precisely worked out last after the PA is brought to a given point in space. However, during operation in the hovering mode, the PA unplannedly displaces from its initial position under the influence of currents, sea waves, and also due to force and moment effects from the cable cable and the working manipulator even with its automatic stabilization system [Filaretov V.F. and other control systems underwater robots. - M .: All year round, 2001, p.171-179; 223-227], which has limited accuracy and inertia. This makes it difficult to automatically perform many manipulation operations. As a result, the problem arises of using additional automatic correction of the programmed trajectory of the working body of the manipulator in the associated SC already in the process of its movement. This correction should be based on information about the real angular and linear displacement of the PA relative to its initial position, obtained from navigation and gyroscopic devices, as well as information about the current configuration of the manipulator, providing additional movement of its working body. In this case, the angular and linear displacements of the PA relative to its initial position in the presence of the above automatic stabilization system should not exceed certain limits, depending on the parameters and kinematic scheme of the manipulator. That is, the design of the manipulator in the process of working out the point A moving along the trajectory I should be able to work out the arising displacements of the PA from the initial position.
Система работает следующим образом. На первом, втором и третьем выходах задатчика 1 программных сигналов формируются сигналы: z, у и х соответственно, а на первом, втором и третьем выходах блока 40 навигации - сигналы: Δх, Δу и Δz соответственно. Первые положительные (со стороны задатчика 1) и вторые отрицательные входы сумматоров 2, 3 и 39 имеют единичные коэффициенты усиления. В результате на выходах сумматоров 2, 3 и 39 формируются сигналы: (z-Δz), (у-Δу) и (х-Δх) соответственно.The system operates as follows. At the first, second and third outputs of the
На первом, втором и третьем выходах блока 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).At the first, second, and third outputs of block 36 of gyroscopes, signals α, β, and γ are formed, respectively. Signals are generated at the outputs of blocks 5, 13, 18, 23, 32, 34, and 9: SγCα, SβSγ, SγCβ, CβCγ, CαCγ, SβCγ and SαSγCβ, respectively, at the output of block 14, the signal SαSβSγ, and at the output of block 6, the signal SγCα (z-Δz), where Sα = sinα; Sβ = sinβ; Cα = cosα, Cβ = cosβ, Cγ = cosγ. Since the positive inputs of the
На выходе блоков 28, 37 и 38 формируются сигналы Sβ(x-Δx), Сβ(у-Δу) и Sα(z-Δz) соответственно. Первый отрицательный (со стороны блока 28) и второй положительный входы сумматора 29 имеют единичные коэффициенты усиления, поэтому на выходе блока 30 формируется сигнал Cα(Cβ(у-Δу)-Sβ(x-Δx)). Положительные входы сумматора 31 имеют единичные коэффициенты усиления. В результате на его выходе формируется сигнал γ*=Сα(Сβ(у-Δу)-Sβ(x-Δх))+Sα(z-Δz).At the output of blocks 28, 37 and 38, signals Sβ (x-Δx), Cβ (y-Δy) and Sα (z-Δz) are formed, respectively. The first negative (from the side of block 28) and the second positive inputs of the adder 29 have unit gains, therefore, the signal Cα (Cβ (у-Δу) -Sβ (x-Δx)) is generated at the output of block 30. The positive inputs of the adder 31 have unity gain. As a result, the signal γ * = Сα (Сβ (у-Δу) -Sβ (x-Δх)) + Sα (z-Δz) is formed at its output.
На выходе блоков 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 имеют единичные коэффициенты усиления, в результате на его выходе формируется сигналAt the output of blocks 24, 33 and 35, signals SαCβCγ, CαCγ (z-Δz) and SαSβCγ are generated, respectively. Since the first negative (from the side of block 24) and second positive inputs of the adder 25, as well as all positive inputs of the adder 19, have unity gains, the signal (SβSγ-SαCβCγ) (у-Δу) is generated at the output of the block 26, and the output of the block 20 - signal (SγCβ + SαSβCγ) (x-Δx). All positive moves of the adder 21 have unity gain, as a result, a signal is generated at its output
z*=(SγCβ+SαSβCγ)(х-Δx)+(SβSγ-SαCβCγ)(у-Δу)+CαCγ(z-Δz).z * = (SγCβ + SαSβCγ) (x-Δx) + (SβSγ-SαCβCγ) (у-Δу) + CαCγ (z-Δz).
Очевидно, что элементы вектора P*(t) в СК XYZ можно получить из разности P(t)-PC(t), а в СК X*Y*Z* - с помощью выраженияObviously, the elements of the vector P * (t) in SC XYZ can be obtained from the difference P (t) -P C (t), and in SC X * Y * Z * using the expression
где R∈R3×3 - матрица поворотов СК X*Y*Z* относительно СК XYZ, Т - символ транспонирования.where R∈R 3 × 3 is the rotation matrix of SK X * Y * Z * relative to SK XYZ, T is the transpose symbol.
Для составления матрицы R необходимо представить поворот ПА вместе с СК X*Y*Z* в виде последовательности элементарных поворотов. Оси, относительно которых отсчитываются углы соответствующих поворотов СК X*Y*Z*, и последовательность этих поворотов должны выбираться такими, чтобы получаемые при этом углы α, β и γ действительно соответствовали углам, которые будут измеряться гироскопами блока 36. Это условие выполняется при следующей последовательности элементарных поворотов СК X*Y*Z*: вначале осуществляется поворот на угол β вокруг оси Z (ему соответствует матрица элементарного поворота RZ,β), затем - поворот на угол α вокруг повернутой оси Х*' (ему соответствует матрица элементарного поворота RX*',α), и, наконец, поворот на угол γ вокруг оси Y* (ему соответствует матрица элементарного поворота RY*,γ). Описанная последовательность поворотов показана на фиг.3. Указанные матрицы поворотов имеют стандартный видTo compile the matrix R, it is necessary to represent the rotation of PA along with SC X * Y * Z * in the form of a sequence of elementary rotations. The axes relative to which the angles of the corresponding rotations of the SC X * Y * Z * are counted, and the sequence of these rotations should be chosen so that the angles α, β, and γ obtained in this case really correspond to the angles that will be measured by the gyroscopes of block 36. This condition is satisfied at the following sequence of elementary rotations SC X * Y * Z *: initially carried rotation through an angle β about the Z axis (it corresponds to an elementary rotation matrix R Z, β), then - turn through an angle α around the axis of the rotated X '' (it corresponds ma Rizza elementary rotation R X * ', α), and finally on the angle of rotation around the Y * γ axis (it corresponds to an elementary rotation matrix R Y *, γ). The described sequence of turns is shown in figure 3. The indicated rotation matrices have a standard form
Учитывая выражения (2), несложно получить матрицу R для описанной последовательности поворотов СК X*Y*Z* вместе с ПАConsidering expressions (2), it is not difficult to obtain the matrix R for the described sequence of rotations of SC X * Y * Z * together with PA
После подстановки транспонированной матрицы R (3) в выражение (1) будем иметьAfter substituting the transposed matrix R (3) into expression (1), we have
Очевидно, что все три элемента вектора P*(t)∈R3 формируются на соответствующих выходах предложенной системы (см. фиг.1). Этот вектор позволяет так управлять рабочим органом манипулятора, чтобы он продолжал двигаться по желаемой траектории при существовании неизбежных текущих отклонений ПА от его исходного положения, для которого и рассчитывается желаемая пространственная траектория движения этого рабочего органа.Obviously, all three elements of the vector P * (t) ∈R 3 are formed at the corresponding outputs of the proposed system (see figure 1). This vector allows controlling the working body of the manipulator so that it continues to move along the desired trajectory in the presence of inevitable current deviations of the PA from its initial position, for which the desired spatial trajectory of movement of this working body is calculated.
Claims (1)
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| RU2011135581/08A RU2462745C1 (en) | 2011-08-25 | 2011-08-25 | System for adjusting motion path of manipulator |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| RU2011135581/08A RU2462745C1 (en) | 2011-08-25 | 2011-08-25 | System for adjusting motion path of manipulator |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| RU2462745C1 true RU2462745C1 (en) | 2012-09-27 |
Family
ID=47078599
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| RU2011135581/08A RU2462745C1 (en) | 2011-08-25 | 2011-08-25 | System for adjusting motion path of manipulator |
Country Status (1)
| Country | Link |
|---|---|
| RU (1) | RU2462745C1 (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| RU2524052C1 (en) * | 2013-01-10 | 2014-07-27 | Федеральное Государственное Автономное Образовательное Учреждение Высшего Профессионального Образования "Дальневосточный Федеральный Университет" (Двфу) | Method for providing navigation of self-contained underwater robot |
| WO2018145917A1 (en) * | 2017-02-13 | 2018-08-16 | Robert Bosch Gmbh | Device and method for regulating the driving operation of remote-controlled vehicle by means of a manipulator |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| SU1352457A1 (en) * | 1986-07-10 | 1987-11-15 | Дальневосточный политехнический институт им.В.В.Куйбышева | Programmed control device with program correction |
| GB2315138A (en) * | 1996-07-05 | 1998-01-21 | Fuji Heavy Ind Ltd | Flight control system for an airplane |
| RU2136034C1 (en) * | 1994-01-26 | 1999-08-27 | Научно-Исследовательский Институт Приборостроения | Method and device for forming of on-board guidance trajectory |
| RU2206916C2 (en) * | 2001-01-04 | 2003-06-20 | Государственное унитарное предприятие "Конструкторское бюро приборостроения" | Method and device for controlling object motion path, object motion path control system (alternatives), method for determining channel-to- channel channels phase connection and transfer constant of object in object motion path control system |
| US20100168919A1 (en) * | 2006-03-24 | 2010-07-01 | Matsushita Electric Industrial Co, Ltd. | Control method and control system for manipulator |
-
2011
- 2011-08-25 RU RU2011135581/08A patent/RU2462745C1/en not_active IP Right Cessation
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| SU1352457A1 (en) * | 1986-07-10 | 1987-11-15 | Дальневосточный политехнический институт им.В.В.Куйбышева | Programmed control device with program correction |
| RU2136034C1 (en) * | 1994-01-26 | 1999-08-27 | Научно-Исследовательский Институт Приборостроения | Method and device for forming of on-board guidance trajectory |
| GB2315138A (en) * | 1996-07-05 | 1998-01-21 | Fuji Heavy Ind Ltd | Flight control system for an airplane |
| RU2206916C2 (en) * | 2001-01-04 | 2003-06-20 | Государственное унитарное предприятие "Конструкторское бюро приборостроения" | Method and device for controlling object motion path, object motion path control system (alternatives), method for determining channel-to- channel channels phase connection and transfer constant of object in object motion path control system |
| US20100168919A1 (en) * | 2006-03-24 | 2010-07-01 | Matsushita Electric Industrial Co, Ltd. | Control method and control system for manipulator |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| RU2524052C1 (en) * | 2013-01-10 | 2014-07-27 | Федеральное Государственное Автономное Образовательное Учреждение Высшего Профессионального Образования "Дальневосточный Федеральный Университет" (Двфу) | Method for providing navigation of self-contained underwater robot |
| WO2018145917A1 (en) * | 2017-02-13 | 2018-08-16 | Robert Bosch Gmbh | Device and method for regulating the driving operation of remote-controlled vehicle by means of a manipulator |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10688659B2 (en) | Robot | |
| US9149931B2 (en) | Robot system, robot control device and method for controlling robot | |
| US11364630B2 (en) | Method for controlling an industrial robot during lead-through programming of the robot and an industrial robot | |
| EP2703131A2 (en) | Robot | |
| EP3377948A1 (en) | Facilitating robot positioning | |
| CN109189060B (en) | Point stabilization control method and device for mobile robot | |
| KR102093916B1 (en) | Electro-optical targetting pod | |
| CN108733066B (en) | Target tracking control method based on pod attitude feedback | |
| CN115502966B (en) | Variable admittance control method for robot | |
| US20160368142A1 (en) | Control device, robot system and method of generating control data | |
| RU2462745C1 (en) | System for adjusting motion path of manipulator | |
| CN107553485B (en) | Method for generating dynamic virtual clamp in human-computer interaction process | |
| Dong et al. | Predictive visual servo kinematic control for autonomous robotic capture of non-cooperative space target | |
| JP5168281B2 (en) | Multi-degree-of-freedom stage controller | |
| Garcia et al. | Vision-driven walking pattern generation for humanoid reactive walking | |
| Zheng-Cai et al. | Predictive control for visual servo stabilization of nonholonomic mobile robots | |
| CN113474130A (en) | Method and system for executing preset task by using robot | |
| Ahmed et al. | Stabilization and control of autonomous hexacopter via visual-servoing and cascaded-proportional and derivative (PD) controllers | |
| Medvedev et al. | Development of AUV path planner based on unstable mode | |
| JP2017030106A (en) | Robot control device and robot system | |
| Wang et al. | Path following control algorithms implemented in a mobile robot with omni wheels | |
| Mihelj et al. | yControl-open architecture controller for Yaskawa Motoman MH5 robot | |
| Dentler | Real-time Model Predictive Control for Aerial Manipulation | |
| KR102093913B1 (en) | Line of sight moving controller of electro-optical targetting pod for joystick command | |
| US20230302642A1 (en) | Systems and Hybrid Position Force Control Processes of an Industrial Robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| MM4A | The patent is invalid due to non-payment of fees |
Effective date: 20180826 |