Устройство позиционно-траекторного управления мобильным роботом

 

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

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

Общая процедура конструирования систем управления мобильным роботом (МР) предполагает организацию его целенаправленного перемещения и сводится к оперативному решению как минимум трех задач: адекватного восприятия информации о внешней среде и внутреннем состоянии; планирования оптимальной траектории перемещения на плане среды в сложившейся внешней ситуации; эффективного исполнения роботом спланированных действий в реальной среде [Каляев А.В., Носков В. П. , Чернухин Ю.В. Алгоритм управляющей структуры транспортного робота. //Известия АН СССР. Техническая кибернетика. - 1980. - 4, с. 64-72]. Таким образом, структурная организация мобильного робота предполагает наличие сенсорной подсистемы, включающей датчики внешней информации (лазерные дальномеры, системы навигации, телевизионные камеры, сонары и т.п.); блока датчиков внутренней информации (датчики углов поворота исполнительных устройств, датчики угловых скоростей колес, осей и т.п.); блока планирования перемещений в пространстве внешних координат в соответствии с выбранным критерием (нейросети, системы принятия решений, системы знаний, программные траектории и т.д.); механической системы робота (тележка, планер, подводный аппарат и т.п.), реализующей его несущие свойства; исполнительных устройств-рабочих органов (двигатели с различным принципом действия, толкатели и т. п. ), осуществляющих перемещение механической системы и воздействие на внешнюю среду и блока тактического управления (уровень жесткого управления), обеспечивающего отработку спланированных траекторий, посредством синтезированных алгоритмов управления [Амосов Н.М. и др. Нейрокомпьютеры и интеллектуальные роботы. Под ред. Н. М.Амосова, Ин-т кибернетики АН УССР. Киев, Наукова думка, 1991, 272 с.].

В дальнейшем набор координат, однозначно описывающий положение мобильного робота в пространстве внешних (связанных, базовых, инерциальных, абсолютных) координат (положение центра тяжести мобильного робота в системе внешних координат, ориентация осей связанной с ним системы координат относительно осей внешней системы) будем называть вектором внешних координат y и предполагать измеряемость всех его элементов, а набор непосредственно управляемых координат (скорость вращения ведущих колес, угол поворота ведомого колеса, положение исполнительных механизмов и т.п.) будем называть вектором внутренних координат z и также предполагать измеряемость всех его элементов.

Известны алгоритм кинематического управления мобильными роботами и соответствующая им структурная схема [L.E. Aguilar, P. Soueres, М. Courdesses, S. Fleury "Robust Path-Following Control with Exponential Stability for Mobile Robots". Proc. of the IEEE Inter. Confer, on Robotics and Automation. Leuven, Belgium, May 1998, p.p. 3279-3283], которая предполагает наличие блока планирования перемещения робота в пространстве внешних координат, блок определения знака линейной скорости, блок вычисления тригонометрических функций, два блока суммирования и три блока перемножения.

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

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

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

Известен алгоритм управления мобильным роботом с автомобильной конструкцией тележки [М. Egersted, X. Hu, A. Stotsky. "Control of a Car-Like Robot Using a Dynamic Model". Proc. of the IEEE Inter. Confer. on Robotics and Automation. Leuven, Belgium, May 1998, p.p. 3273-3278], который предполагает наличие блока планирования перемещений в пространстве внешних координат, блока датчиков внутренней информации, сенсорной подсистемы, блока вычисления тригонометрических функций, блока суммирования и блока перемножения.

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

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

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

Известен экспоненциальный алгоритм управления [O.J. Sordalen, С. Canudas de Wit. "Exponential Control Low for a Mobile Robot: Extension to Path Following". IEEE Transaction on Robotics and Automation, vol. 9, 6, 1993, p. p. 837-842] , который предполагает наличие блока планирования перемещений в пространстве внешних координат, блока датчиков внутренней информации, сенсорной подсистемы, блоков вычисления тригонометрических функций, блоков суммирования и блоков перемножения.

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

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

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

Известна структура устройства управления мобильным роботом [С.Ф.Бурдаков, Р.Э.Стельмаков, С.В.Штайнле. Синтез траекторий и управление мобильными роботами в условиях неопределенности. Материалы 8-й научно-технической конференции "Экстремальная робототехника". Под научной редакцией проф. Е.И. Юревича. СПб, СПбГТУ, 1997, 439 с., с.с. 198-209], который предполагает наличие блока планирования перемещений в пространстве внешних координат, блока датчиков внутренней информации, сенсорной подсистемы, блока аппроксимации (подсистема прокладки траектории), регулятора положения, регулятора скорости, блока адаптации, блока ограничений, блока кинематических преобразований, блоков сравнения, расчетных блоков.

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

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

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

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

Технический результат, достигаемый при осуществлении предлагаемого устройства, заключается в том, что предлагаемые алгоритмы управления не требуют наличия блока кинематических преобразований, не требуется наличия аппроксимирующих устройств и интерполяторов в структуре системы управления роботом, что позволяет исключить соответствующие составляющие погрешности задания, связанные с приближенностью расчетов [С.Ф.Бурдаков, В.А.Дьяченко, А.В.Тимофеев. Проектирование манипуляторов промышленных роботов и роботизированных комплексов. М., Высшая школа, 1986, 246 с., с. 60] и, следовательно, снизить погрешность отработки планируемых траекторий движения, т.е. повысить точность движения робота. Расширение функциональных возможностей заключается в том, что предлагаемое устройство в зависимости от поставленной задачи позволяет организовать движение из произвольной точки фазового пространства внешних координат в заданную, с нулевой скоростью; обеспечить движение робота вдоль траекторий во всем классе квадратичных форм с заданной траекторной скоростью; осуществить перемещение робота вдоль заданной траектории в заданную точку без предъявления дополнительных требований к траекторной скорости. Свойство асимптотической устойчивости в целом планируемых фазовых траекторий, гарантируемое предлагаемым устройством, позволяет предположить малую чувствительность замкнутых систем к изменению параметров объекта и к внешним возмущениям определенного класса [А.А. Колесников. Синергетическая теория управления. М.- Таганрог, 1994, 344 с., с. 261].

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

Теоретическое доказательство наличия причинно-следственной связи между заявляемыми признаками и достигаемым техническим результатом приведено ниже.

Пусть мобильный робот описывается своими динамической и неголономной кинематической моделями вида где z - n-вектор внутренних, управляемых и наблюдаемых координат; F - n-вектор нелинейных элементов; В - невырожденная (nхn)-матрица коэффициентов управления; U - n-вектор управлений; y - вектор наблюдаемых внешних координат, dim y=m, m≥n, m≤6; М - вектор-функция внешних скоростей; r - конструктивные параметры.

Структуры вектора F(z) и матрицы В(z) однозначно определяются для каждого конкретного робота в результате процедуры вывода математической модели робота. Причем, как правило, для большинства моделей функциональные элементы представленного вектора и матрицы являются функциями внутренних координат (угол поворота рулевого колеса, скорость вращения ведущих колес и т.п.) и определяются динамикой механической системы мобильного робота, исполнительных механизмов и дополнительного оборудования (манипуляторы, толкатели и т. п. ). Структура этих векторов и матриц для конкретных мобильных роботов приведены, например, в следующих работах: Бурдаков С.Ф., Стельмаков Р.Э., Штайнле С. В. Синтез траекторий и управление мобильными роботами в условиях неопределенности. Материалы 8-й научно-технической конференции "Экстремальная робототехника". Под научной редакцией проф. Е.И.Юревича. СПб. , СПбГТУ, 1997, 439 с., с.с. 198-209 - используется линейное представление элементов вектора F(z), а элементы матрицы B(z) рассматриваются как константы; Мирошник И.В., Говядинкин Д.С., Дроздов В.Н. Система управления транспортной тележкой", Управление в оптических и электромеханических системах. Л., ЛИТМО, 1989 - элементы вектора F(z) тождественно равны нулю, а матрица B(z) является единичной и диагональной: Девянин Е.А. О движении колесных роботов. В сборнике докладов научной школы-семинара "Мобильные роботы и мехатронные системы", М., 1998, с.с. 169-200 - рассматривается нелинейное представление как элементов вектора F(z), так и элементов матрицы B(z). Более общая структура уравнений динамики неголономных систем, модифицированный вариант которой представлен уравнением (1), приведен в известной работе Bloch A.M., Reyhanoglu М. , McClamroch M.H. "Control and Stabilization of Nonholonomic Dynamic Systems", IEEE Transactions on Automatic Control, v. 37, 11, 1992, p.p. 1746-1757.

Например, представленными моделями (1) и (2) описывается ряд мобильных роботов на базе транспортных тележек. Как правило, для мобильного робота на базе транспортной тележки n= 2 (одно уравнение описывает динамику ведущих колес, другое - динамику рулевого (рулевых) колес), m=3 (два уравнения описывают положение центра тяжести робота по координатам y₁ и y₂, третье - угол ориентации у₃ корпуса в системе внешних координат) и от элементов вектора у не зависит [С. Ф.Бурдаков, Р.Э.Стельмаков, С.В.Штайнле. Синтез траекторий и управление мобильными роботами в условиях неопределенности. Материалы 8-й научно-технической конференции "Экстремальная робототехника". Под научной редакцией проф. Е.И. Юревича. СПб, из-во СПбГТУ, 1997, 439 с., с.с. 198-209].

Пусть матрица Якоби кинематической модели (2)

являющаяся производной вектор-столбца внешних координат по вектор-строке внутренних координат, имеет ранг, равный n, т.е. rankR=n, здесь M^*=(M₁,M₂, . .. M_n)^T.

Рассмотрим задачу контурного управления мобильным роботом, когда необходимо организовать движение робота вдоль заданной траектории с заданной траекторной (контурной) скоростью V_k [Пшихопов В.Х., Чернухин Ю.В. Контурный регулятор для нейросетевой системы управления адаптивного мобильного робота. В сб. трудов международной научно-технической конференции "Интеллектуальные многопроцессорные системы", Таганрог, 1999, с. 210-217.].

Пусть желаемые траектории движения мобильного робота на плоскости описываются квадратичными формами от внешних координат y₁ и y₂ вида
a₁₁ ¹y₁ ²+ a₂₂ ¹y₂ ²+ a₃₁y₁+ a₄₁y₂+a₅₁=0,
или в матричной форме

здесь
Уравнениями вида (4) в зависимости от матриц могут быть описаны окружности, эллипсы, прямые и т.д.

Сформируем требования к скорости движения МР вдоль траекторий (4) в виде скоростного многообразия

где V_k - определяемая заданием контурная скорость, а скорость движения МР относительно траектории (4) - равенством

Очевидно, что значение контурной скорости V_k должно удовлетворять неравенству
V_k≤V_max, (7)
где V_max - максимальная контурная скорость, определяемая энергетическими возможностями приводов МР.

Тогда траекторное Ψ_T и скоростное Ψ_C подмногообразия могут быть сформированы в виде векторов [Пшихопов В.Х., Чернухин Ю.В. Контурный регулятор для нейросетевой системы управления адаптивного мобильного робота. В сб. трудов международной научно-технической конференции "Интеллектуальные многопроцессорные системы", Таганрог, 1999, с. 210-217.]:

и, соответственно, желаемое многообразие Ψ, отражающее требование к движению МР в его фазовом пространстве, может быть представлено их линейной комбинацией в следующей векторной записи

где А - положительно определенная матрица постоянных задаваемых коэффициентов; dim А=(nхn); 0₁-(1хn) - вектор нулевых элементов; T- символ транспонирования.

При повышении размерности пространства функционирования МР траекторное многообразие Ψ_T дополняется многообразиями вида (4), описывающими дополнительные ограничения на траекторию движения робота, до размерности n-1. В качестве дополнительных многообразий могут быть заданы ограничения на углы ориентации робота. Соответственно, в силу выражений (7) и (8) будет увеличена и размерность скоростного многообразия Ψ_C.
Для того чтобы многообразия (9) трансформировать в аттракторы, зададим желаемую динамику замкнутой системы в виде дифференциального уравнения вида [А.А.Колесников., Синергетическая теория управления. М.- Таганрог, 1994, 344 с., с. 66]:

где С - положительно определенная (nхn)-матрица задаваемых постоянных коэффициентов.

Очевидно, что решение уравнения (10)

где Ψ₀ - отклонение от многообразия (9) в начальный момент времени t₀∈t, здесь t - время решения поставленной перед МР задачи, сформулированной многообразием (9).

Подставив модель МР (1), (2) и выражения (9) в уравнение (10) и решив последнее относительно вектора управляющих воздействий U, получим [V. Pshikhopov, Y. Chemukhin "Path Following Regulator for Neural Network Implemented Control System of Adaptive Mobile Robot Moving with a Set Speed". In Proc. of Int. Confer. "Mathematical Theory of Networks and Systems", June 18-23, 2000, Perpignan, France]


где D₁= 2y^тN₁ ¹+N₂ ¹, C, A, N₁ ¹,N₂ ¹,N₃ ¹, V - матрицы и векторы постоянных коэффициентов соответствующей размерности, L - матрица Якоби кинематической модели (2), являющаяся производной вектор-столбца внешних скоростей по вектор-строке внешних координат у.

Алгоритм (12) позволяет организовать движение МР вдоль желаемых траекторий, задаваемых квадратичными формами внешних координат у*, с матричными коэффициентами N₁ ¹,N₂ ¹,N₃ ¹, с постоянной контурной скоростью V_k. Причем значение контурной скоростью V_k определяется, с одной стороны, требованиями к производительности МР, с другой - энергетическими возможностями приводов МР.

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

Сформируем теперь целевые функции, соответствующие поставленной задаче, в виде следующих равенств:

где (nх1)-векторы соответственно координат точки позиционирования и значений в ней внешней скорости.

Соответственно, траекторные Ψ_T и скоростные Ψ_C многообразия могут быть представлены в следующем виде:

а их линейная комбинация

где А - (nхn)-матрица, имеющая тот же смысл, что и в выражении (9), является линией пересечения требований (15) в фазовом пространстве управляемого объекта.

Для того чтобы многообразия (16) трансформировать в аттракторы, зададим желаемую динамику замкнутой системы в виде дифференциального уравнения вида (10).

Подставив модель МР (1), (2) и выражения (15), (16) в уравнение (10) и, решив последнее относительно вектора управляющих воздействий U, получим

где матрицы R, L, М^* и М имеют тот же смысл, что и в алгоритме (12).

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

Очевидно, что управления (12) и (17) имеют общую структуру. Причем управление (17) является частным случаем алгоритма (12).

Отмеченная структурная общность алгоритмов позиционного (17) и контурного (12) управлений позволяет разработать общие принципы построения соответствующих систем управления. Кроме того, обобщенная структура регулятора позволяет построить и эффективные алгоритмы позиционно-контурного управления, когда требуется перевести МР из произвольной точки пространства внешних координат в заданную, вдоль желаемой траектории, без предъявления дополнительных требований к контурной скорости.

В любой из представленных постановок задач представляется целесообразным, исходя из выше изложенного подхода, формировать траекторные Ψ_T и скоростные Ψ_C многообразия в виде следующих векторов:


здесь n - размерность вектора управлений, M_e - (nхn)-матрица, diagM_e=e; D_i=2y^тN₁ ⁱ+N₂ ⁱ- вспомогательная матрица соответствующей размерности, i=1,n.

При решении задач контурного управления, в общем случае, N_j ⁱ≠0, N₂ ^j≠0, N₃ ^j= 0, N₁ ⁿ= 0, N₂ ⁿ=0, N₃ ⁿ=0, M_e=E, V_k≠0, где Е - единичная матрица cоответствующей размерности, и выражения для траекторных и скоростных многообразий, построенных по выражению (18), будут совпадать с представленными в выражении (8).

При решении задач позиционного управления N₁ ⁱ=0, N₂ ⁱ=Е, Nⁱ₃ = y^*_fi, M_e=0, V_k= 0 и выражения для траекторных и скоростных многообразий, построенных по выражению (18) будут совпадать с представленными в выражении (15).

В случае решения позиционно-контурной задачи, Σ_j, может задавать желаемую траекторию движения, a Σ_n - одну из координат точки позиционирования. При этом M_e=0, V_k=0.

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

Используя изложенные выше процедуры формирования желаемых многообразий в пространстве внешних координат и их трансформацию в аттракторы, получим следующий алгоритм позиционно-траекторного управления [В.Х.Пшихопов. "Аналитический синтез синергетических регуляторов для позиционно-траекторных систем управления мобильными роботами". Материалы 11-й научно-технической конференции "Экстремальная робототехника". Под научной редакцией проф. Е.И. Юревича. СПб., СПбГТУ, 2000]

где индексы i, j, n имеют тот же смысл, что и в выражении (18).

Требуемое условие положительной определенности матриц С и А выполняется, в частности, при diagC=diagA=s_i>0.

Следует обратить внимание на условие rank(k₁RB)=n, нарушение которого, в силу свойств задаваемых матриц С и А, а также предлагаемой управляемости МР и условия (3), возможно при нахождении МР в характерных точках планируемых траекторий (центр окружности, фокусы эллипса, бесконечно удаленные точки, принадлежащие биссектрисе параболы и т. п.), а также при одновременном обнулении внешних скоростей (только для контурной задачи) [Пшихопов В.Х., Колесников А.А. Устройство контурного управления манипуляционным роботом. Патент РФ 2146606, бюл. 8, 2000]. Такие случаи возможны только в момент начала движения. При этом моменты приводов МР могут выходить на свои ограничения, что не всегда желательно при организации плавного движения МР. Последнее обстоятельство определяет необходимость применения "выталкивающих" процедур. В частности, при нахождении МР в указанных точках возможно использование позиционного алгоритма управления, с последующим переходом, в зависимости от поставленной задачи, на контурные или позиционно-контурные алгоритмы. Возможность использования в таких ситуациях позиционных алгоритмов определяется отсутствием неопределенности управляющих воздействий во всех точках фазового пространства.

Преимущества по сравнению с прототипом, достигаемые при осуществлении предлагаемого устройства, заключаются в том, что предлагаемые алгоритмы управления не требуют наличия блока кинематических преобразований, не требуется наличия аппроксимирующих устройств и интерполяторов в структуре системы управления роботом, что позволяет исключить соответствующие составляющие погрешности задания, связанные с приближенностью расчетов [С.Ф.Бурдаков, В. А. Дьяченко, А.В.Тимофеев Проектирование манипуляторов промышленных роботов и роботизированных комплексов. М. , Высшая школа, 1986, 246 с., с. 60] и, следовательно, снизить погрешность отработки планируемых траекторий движения, т.е. повысить точность движения робота. Расширение функциональных возможностей заключается в том, что предлагаемое устройство в зависимости от поставленной задачи позволяет организовать движение из произвольной точки фазового пространства внешних координат в заданную, с нулевой скоростью; обеспечить движение робота вдоль траекторий во всем классе квадратичных форм с заданной траекторной скоростью; осуществить перемещение робота вдоль заданной траектории в заданную точку без предъявления дополнительных требований к траекторной скорости.

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

Изобретение поясняется чертежами, где на фиг.1.1, 1.2 представлена блок-схема мобильного робота, замкнутого синтезированным устройством позиционно-траекторного управления; на фиг.2 - блок-схема алгоритма функционирования устройства; на фиг.3 - диаграмма работы устройства.

Устройство позиционно-траекторного управления мобильным роботом, представленное на фиг.1, содержит блок 1 планирования траекторий (ПТ) в пространстве внешних координат, первые выходы которого подключены на первые входы блока перемножения (П) 2, а его n-1 вторых выходов - на первые входы блока суммирования (С) 3, выходы которого связаны со вторыми входами блока суммирования 4, первые входы которого подключены к выходам блока перемножения 2, а его выходы - к первым инверсным входам блока суммирования 5.

(n-l)n третьих выходов блока 1 планирования траекторий в пространстве внешних координат подключены ко входам блока 6 умножения на два (БУД) и первым входам блока перемножения 7, а выходы блока 2 умножения на два подключены к первым входам блока перемножения 8 и первым входам блока перемножения 9. Выходы блока перемножения 8 соединены с первыми входами блока суммирования 10, а выходы блока перемножения 7 - с первыми входами блока суммирования 11, вторые входы которого связаны с четвертыми (n-1)n выходами блока 1 планирования траекторий в пространстве внешних координат и вторыми входами блока суммирования 10, а его выходы подключены к первым входам блока перемножения 12, выходы которого связаны со вторыми входами блока суммирования 3.

Пятые n выходов блока 1 планирования траекторий в пространстве внешних координат подключены к первым входам блока перемножения 13, вторым входам блока суммирования 14 и вторым входам блока перемножения 15, первые входы которого соединены с выходами блока суммирования 10 и первыми входами блока перемножения 16, третьи входы - с шестыми n выходами блока 1 планирования траекторий в пространстве внешних координат, вторыми входами блока суммирования 14, выход которого подключен ко вторым входам блока перемножения 16, со вторыми входами блока перемножения 2 и вторыми входами блока перемножения 13, третьи входы которого подключены к выходам блока перемножения 9.

Седьмые n выходов блока 1 планирования траекторий в пространстве внешних координат подключены к первым входам блока суммирования 17, выходы которого связаны с четвертыми входами блока перемножения 15, к первым входам блока суммирования 18, выходы которого связаны с третьими входами блока перемножения 16, и к первым входам блока суммирования 19, выходы которого связаны с первыми входами блока перемножения 20, вторые входы которого подключены к первым входам блока 1 планирования траекторий в пространстве внешних координат, к входам блока 21 транспонирования матриц (БТ), выходы которого связаны со вторыми входами блока перемножения 8 и вторыми входами блока перемножения 7, к входам блока 22 транспонирования матриц, ко вторым входам блока перемножения 12, вторым входам блока 23 вычисления вектора внешних скоростей (БВС), вторым входам блока 24 вычисления производной вектор-столбца внешних скоростей по вектор-строке внутренних координат (ЯВH), вторым входам блока 25 вычисления производной вектор-столбца внешних скоростей по вектор-строке внешних координат (ЯВК), m выходам сенсорной подсистемы (СП) 26, входы которой связаны с внешней средой.

Восьмые n выходов блока 1 планирования траекторий в пространстве внешних координат подключены к первым входам блока перемножения 27, вторые входы которого связаны с выходами блока 28 транспонирования матриц, а выходы - со вторыми входами блока суммирования 17.

Девятые n выходов блока 1 планирования траекторий в пространстве внешних координат подключены к первым входам блока 29 умножения на 2, выходы которого соединены со вторыми входами блока суммирования 18, а его вторые входы - с выходами блока 22 транспонирования матриц и вторыми входами блока перемножения 30, к первым входам блока перемножения 30, выходы которого соединены со вторыми входами блока суммирования 19.

Десятый выход блока 1 планирования траекторий в пространстве внешних координат подключен ко второму входу блока суммирования 31, первые входы которого связаны с выходами блока перемножения 20, а его выходы соединены с третьими входами блока суммирования 4.

Выходы блока 23 вычисления вектора внешних скоростей подключены ко вторым входам блока перемножения 9, входам блока 32 умножения на 2, выходы которого связаны со входами блока 28 транспонирования матриц, и ко вторым входам блока перемножения 33, первые входы которого связаны с выходами блока суммирования 34, а его выходы - с третьими инверсными входами блока суммирования 5, вторые входы которого подключены к выходам блока перемножения 35.

n выходов блока 36 вычисления вектора нелинейных элементов (БНЭ) подключены к третьим входам блока перемножения 35, вторые входы которого связаны с выходами блока 24 вычисления производной вектор-столбца внешних скоростей по вектор-строке внутренних координат (ЯВН) и вторыми входами блока перемножения 37, первые входы которого соединены с выходами блока перемножения 15, первыми входами блока перемножения 35 и первыми входами блока перемножения 38, вторые входы которого подключены к выходам блока 25 вычисления производной вектор-столбца внешних скоростей по вектор-строке внешних координат.

Выходы блока 39 вычисления матрицы коэффициентов управления (БКУ) подключены к третьим входам блока перемножения 37, выходы которого подключены ко входам блока 40 обращения матриц (БОМ), выходы которого соединены со вторыми входами блока перемножения 41, первые входы которого подключены к выходам блока суммирования 5, а его выходы - к n входам блока 42 исполнительных устройств (ИУ).

Выходы блока 42 исполнительных устройств подключены ко входам механической системы (МС) 43, выходы которой связаны с внешней средой, и ко входам блока 44 датчиков внутренней информации (БДВИ), выходы которого связаны с входами блока 36 вычисления вектора нелинейных элементов, входами блока 39 вычисления матрицы коэффициентов управления, первыми входами блока 25 вычисления производной вектор-столбца внешних скоростей по вектор-строке внешних координат, первыми входами блока 24 вычисления производной вектор-столбца внешних скоростей по вектор-строке внутренних координат, блока 23 вычисления вектора внешних скоростей и вторыми входами блока 1 планирования траекторий в пространстве внешних координат.

Первые входы блока суммирования 45 связаны с выходами блока перемножения 13, вторые входы - с выходами блока перемножения 16, а выходы подключены к первым входам блока суммирования 34, вторые входы которого связаны с выходами блока перемножения 38.

Под блоком планирования траекторий в пространстве внешних координат понимается совокупность блоков и модулей, формирующих желаемые траектории движения робота. Т.е., в состав этого блока могут быть включены ЭВМ верхнего уровня, пульт оператора, чувствительные устройства и т.п. [Управление роботами от ЭВМ. Под ред. Юревича Е. И.. Л., Энергия, 1980, 264 с., с. 78]. В качестве основного элемента блока планирования траекторий в пространстве внешних координат возможно использование управляющей структуры на нейроподобных элементах [Каляев А.В., Носков В.П., Чернухин Ю.В. Алгоритм управляющей структуры транспортного робота. //Известия АН СССР. Техническая кибернетика. - 1980.- 4, с. 64-72].

Структура блоков суммирования, перемножения и транспонирования матриц (векторов), обращения матриц определяется известными правилами матричного исчисления [Корн Г., Корн Т. Справочник по математике для научных работников и инженеров. М., 1984, 833с., с. 392-393]. Например, блок транспонирования матриц может представлять собой коммутатор с жесткими связями между (n•n) входами и (n•n) выходами, как показано на фиг.5 в [Пшихопов В.Х., Колесников А. А. Устройство контурного управления манипуляционным роботом. Патент РФ 2146606, бюл. 8, 2000]. Нулевые элементы диагональных матриц структурой устройства управления не формируются.

Следует отметить, что структурная реализация блоков 23, 24, 25, 36, 39 однозначно определяется по известной модели (1), (2), аналитическим выражениям (3), (13) для каждого конкретного робота, представляющими собой наборы операций умножения, суммирования, определения тригонометрических функций.

Блок-схема алгоритма функционирования устройства и диаграмма его работы представлены соответственно на фиг.2 и 3.

Устройство позиционно-траекторного управления функционирует следующим образом. Блок планирования траекторий в пространстве внешних координат 1 (см. фиг.1) в зависимости от задания формирует на своих вторых, третьих, четвертых, седьмых, девятых и двенадцатых выходах соответствующие коэффициенты квадратичных форм, являющиеся коэффициентами при квадратах координат - N₁ ^j, N₁ ⁿ, при координатах и свободными константами -N₂ ^j, N₂ ⁿ и N₃ ^j, N₃ ⁿ соответственно. Указанные коэффициенты в силу выражений (18) определяют вид желаемой траектории движения или координаты точки позиционирования. На n пятых и n шестых его выходах формируются диагональные элементы определенно положительных матриц С и А, в силу уравнений (10) и (11) определяющих динамику движения робота к линии пересечения многообразий (18).

Одновременно на первых выходах блока планирования траекторий в пространстве внешних координат 1 формируется требуемое значение траекторной (контурной) скорости V_k движения вдоль заданных траекторий, а также блоком датчиков внутренней информации 44 снимаются показания с n выходов блока 42 исполнительных устройств робота, а сенсорной подсистемой 26, по m входам - координаты положения робота во внешней среде. Кроме того, на n восьмых выходах блока планирования траекторий в пространстве внешних координат 1 формируются значения коэффициентов матрицы M_e.

Причем при решении задач контурного управления, в общем случае, N_i ^j≠0, N₂ ^j≠0, N₃ ^j≠0, N₁ ⁿ= 0, N₂ ⁿ= 0, N₃ ⁿ=0, М_e=Е, V_k≠0, где Е-единичная матрица соответствующей размерности. При решении задач позиционного управления N₁ ⁱ= 0, N₂ ⁱ=Е, Nⁱ₃ = y^*_fi. Для случая решения позиционно-контурной задачи M_e=0, V_k= 0.

Сигналы с n выходов блока датчиков внутренней информации 44, соответствующие вектору z состояния внутренних координат подаются соответственно на входы блока 26 вычисления нелинейных элементов вектора F, на входы блока 39 вычисления матрицы коэффициентов управления В, на первые входы блока 25 вычисления производной вектор-столбца внешних скоростей по вектор-строке внешних координат L по выражению (13), на первые входы блока 24 вычисления производной вектор-столбца внешних скоростей по вектор-строке внутренних координат R по выражению (3), на первые входы блока 23 вычисления вектора внешних скоростей М* по выражению (2), с учетом пояснений к формуле (3). А m сигналов от сенсорной подсистемы 26 поступают на вторые входы блока 25 вычисления производной вектор-столбца внешних скоростей по вектор-строке внешних координат, блока 24 вычисления производной вектор-столбца внешних скоростей по вектор-строке внутренних координат, блока 23 вычисления вектора внешних скоростей и определяют процедуры соответствующих вычислений.

Следует отметить, что структурная реализация блоков 23, 24, 25, 36 и 39 однозначно определяется из известной модели (1), (2) для каждого конкретного мобильного робота.

Расчет матричных коэффициентов k₁, k₂, k₃ осуществляется в соответствии с выражениями, приведенными в (19). Причем на nхn выходах блока перемножения 15 посредством блоков умножения на два 6 и 32, перемножения 8 и 27, суммирования 10 и 17, транспонирования 21 и 28, формируется матричный коэффициент k₁. На n выходах блока суммирования 45 посредством блоков умножения на два 6, перемножения 9, 13 и 16, суммирования 14 и 18, транспонирования 22 формируется матричный коэффициент k₂. На n выходах блока суммирования 4 посредством блоков умножения на два, перемножения 2, 3, 12, 20 и 30, суммирования 3, 11, 19 и 31 формируется матричный коэффициент к₃.

Расчет сигналов управления, в соответствии с алгоритмом (19), осуществляется блоками суммирования 5, 34, обращения матриц 40, перемножения 33, 35, 37, 38, а формирование n сигналов управления U по алгоритму (19)- блоком 41.

Посредством блока 42 исполнительных устройств осуществляется формирование управляющих воздействий на механическую систему 43, которая отрабатывает их. Блок планирования траекторий в пространстве внешних координат 1, снимая показания с блока датчиков внутренней информации 44 и сенсорной подсистемы 26, определяет необходимость дальнейшего формирования управляющих сигналов. В случае, если технологическая задача не решена, блок планирования траекторий в пространстве внешних координат 1 повторно формирует набор тех же констант на своих выходах, которые инициализируют работу остальных блоков устройства (см. фиг.3). В противном случае работа устройства завершается и константы на выходах блока планирования траекторий в пространстве внешних координат 1 не формируются или осуществляется переход к решению следующей технологической задачи, т.е. формированию нового набора констант N₁ ^j, N₂ ^j, N₃ ^j, N₁ ⁿ, N₂ ⁿ, N₃ ^j, C, A, V_k, M_e.

Функционирование блоков суммирования, перемножения, обращения и транспонирования осуществляется по известным правилам матричного исчисления [Корн Г., Корн Т. Справочник по математике для научных работников и инженеров. М., 1984, 833с., с. 392-393].

Расчет управляющих воздействий по алгоритму (19) осуществляется с частотой не менее 50 Гц [М. Вукобратович, Д.Стокич, Н.Кирчански. Неадаптивное и адаптивное управление манипуляционными роботами. М.: Мир, 1998, 376 с., с. 338-339] до полного выполнения технологического задания.

Технический результат, достигаемый при осуществлении предлагаемого устройства, заключается в том, что предлагаемые алгоритмы управления не требуют наличия блока кинематических преобразований, не требуется наличия аппроксимирующих устройств и интерполяторов в структуре системы управления роботом, что позволяет исключить соответствующие составляющие погрешности задания, связанные с приближенностью расчетов [С.Ф.Бурдаков, В.А.Дьяченко, А.В. Тимофеев. Проектирование манипуляторов промышленных роботов и роботизированных комплексов. М., Высшая школа, 1986, 246 с., с. 60] и, следовательно, снизить погрешность отработки планируемых траекторий движения, т.е. повысить точность движения робота. Расширение функциональных возможностей заключается в том, что предлагаемое устройство в зависимости от поставленной задачи позволяет организовать движение из произвольной точки фазового пространства внешних координат в заданную, с нулевой скоростью; обеспечить движение робота вдоль траекторий во всем классе квадратичных форм с заданной траекторной скоростью; осуществить перемещение робота вдоль заданной траектории в заданную точку без предъявления дополнительных требований к траекторной скорости. Свойство асимптотической устойчивости в целом планируемых фазовых траекторий, гарантируемое предлагаемым устройством, позволяет предположить малую чувствительность замкнутых систем к изменению параметров объекта и к внешним возмущениям определенного класса [А.А.Колесников, Синергетическая теория управления. М.- Таганрог, 1994, 344 с., с. 261].

Представленные алгоритмы позиционно-траекторного управления могут быть реализованы программно, с использованием устройств микропроцессорной техники, например высокопроизводительных контроллеров (одного или нескольких) [М.Вукобратович, Д.Стокич, Н. Кирчански. Неадаптивное и адаптивное управление манипуляционными роботами. М.: Мир, 1998, 376 с., с. 320-326], в соответствии с блок-схемой алгоритма, представленной на фиг.2, и диаграммой работы устройства, представленной на фиг.3.

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

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

РИСУНКИ

Рисунок 1, Рисунок 2, Рисунок 3, Рисунок 4, Рисунок 5