ЭЛЕКТРОПРИВОД МАНИПУЛЯТОРА Российский патент 2015 года по МПК B25J13/00 

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

Известно устройство для управления приводом робота, содержащее последовательно соединенные первый и второй сумматоры, первый блок умножения, третий сумматор, первый усилитель, электродвигатель, связанный непосредственно с первым датчиком скорости, выход которого подключен ко второму входу второго сумматора и второму входу третьего сумматора, и через редуктор - с первым датчиком положения, выход которого подключен к первому входу первого сумматора, второй вход которого соединен со входом устройства, последовательно соединенные второй датчик положения, четвертый сумматор, второй вход которого подключен к выходу первого задатчика постоянного сигнала, пятый сумматор, второй вход которого подключен к выходу второго задатчика постоянного сигнала, второй блок умножения, второй вход которого подключен к выходу датчика массы, шестой сумматор и третий блок умножения, последовательно соединенные третий датчик положения, первый косинусный функциональный преобразователь и четвертый блок умножения, второй вход которого подключен к выходу второго датчика скорости, последовательно соединенные первый датчик ускорения, пятый блок умножения, седьмой сумматор, второй вход которого подключен к выходу шестого блока умножения, и седьмой блок умножения, последовательно соединенные третий датчик скорости, квадратор, восьмой блок умножения, второй вход которого подключен ко второму входу пятого блока умножения и через второй синусный функциональный преобразователь - к выходу третьего датчика положения, и восьмой сумматор, последовательно соединенные девятый блок умножения, первый вход которого подключен ко второму входу восьмого блока умножения, а второй вход - к выходу третьего датчика скорости и первому входу шестого блока умножения, десятый блок умножения, и девятый сумматор, второй вход которого подключен к выходу одиннадцатого блока умножения, последовательно соединенные второй датчик ускорения и двенадцатый блок умножения, второй вход которого подключен к выходу первого косинусного функционального преобразователя, а выход - к второму входу восьмого сумматора, последовательно соединенные третий датчик ускорения, установленный на выходном валу редуктора, и десятый сумматор, выход которого подключен к третьему входу третьего сумматора, последовательно соединенные третий задатчик постоянного сигнала и одиннадцатый сумматор, выход которого подключен ко второму входу первого блока умножения, а также релейный элемент, вход которого подключен к выходу первого датчика скорости, а выход - к четвертому входу третьего сумматора, и тринадцатый блок умножения, первый вход которого подключен к выходу восьмого блока умножения, а его второй вход - к выходу второго датчика скорости, последовательно соединенные четвертый задатчик постоянного сигнала, двенадцатый сумматор, второй вход которого подключен к выходу датчика массы, и тринадцатый сумматор, второй вход которого соединен с выходом пятого задатчика постоянного сигнала, а выход - со вторым входом одиннадцатого сумматора и пятым входом третьего сумматора, причем выход двенадцатого сумматора соединен со вторым входом седьмого блока умножения, а выход четвертого задатчика постоянного сигнала - с первым входом четырнадцатого блока умножения, второй вход которого подключен к выходу четвертого сумматора, а выход - второму входу шестого сумматора, последовательно соединенные пятнадцатый блок умножения, первый и второй входы которого соединены с выходами квадратора и третьего датчика скорости, соответственно, и шестнадцатый блок умножения, второй вход которого подключен к выходу первого косинусного функционального преобразователя, а выход - к третьему входу девятого сумматора, последовательно соединенные первый дифференциатор, вход которого подключен к выходу первого датчика ускорения, семнадцатый блок умножения, второй вход которого подключен к выходу второго синусного функционального преобразователя, четырнадцатый сумматор, второй вход которого подключен к выходу тринадцатого блока умножения, третий вход через последовательно соединенные восемнадцатый и девятнадцатый блоки умножения - с выходом третьего датчика скорости, а четвертый вход через двадцатый блок умножения - с выходом второго датчика скорости, и двадцать первый блок умножения, второй вход которого подключен к выходу двенадцатого сумматора, а выход - ко второму входу десятого сумматора, третий вход которого соединен с выходом двадцать второго блока умножения, а также второй дифференциатор, вход которого подключен к выходу второго датчика ускорения и второму входу десятого блока умножения, а выход - к первому входу одиннадцатого блока умножения, второй вход которого соединен с выходом первого косинусного функционального преобразователя и вторым входом девятнадцатого блока умножения, причем вторые входы восемнадцатого и двадцатого блоков умножения соединены, соответственно, с выходами первого датчика ускорения и двенадцатого блока умножения, входы двадцать второго блока умножения соединены с выходами шестого и девятого сумматоров, второй вход третьего блока умножения подключен к выходу восьмого сумматора, а выход - к шестому входу третьего сумматора, выход четвертого блока умножения подключен ко второму входу шестого блока умножения, выход седьмого блока умножения соединен с седьмым входом третьего сумматора (см. патент РФ № Патент 2423224. Бюл. №19, 2011).

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

Известен также самонастраивающийся электропривод робота, содержащий последовательно соединенные первый и второй сумматоры, первый блок умножения, третий сумматор, усилитель, электродвигатель, связанный непосредственно с первым датчиком скорости, выход которого подключен ко вторым входам второго и третьего сумматоров, и через редуктор - с первым датчиком положения, выход которого подключен к первому входу первого сумматора, второй вход которого соединен со входом устройства, последовательно соединенные второй датчик положения, установленный в третьей степени подвижности манипулятора, четвертый сумматор, второй вход которого подключен к выходу первого задатчика постоянного сигнала, пятый сумматор, второй вход которого подключен к выходу второго задатчика постоянного сигнала, второй блок умножения, второй вход которого подключен к выходу датчика массы, шестой сумматор и третий блок умножения, выход которого подключен к третьему входу третьего сумматора, последовательно соединенные третий датчик положения, установленный во второй степени подвижности манипулятора, и первый косинусный функциональный преобразователь, последовательно соединенные второй датчик скорости, установленный в третьей степени подвижности манипулятора, и четвертый блок умножения, последовательно соединенные первый датчик ускорения, установленный в третьей степени подвижности манипулятора, пятый блок умножения, седьмой сумматор, второй вход которого подключен к выходу шестого блока умножения и седьмой блок умножения, последовательно соединенные третий датчик скорости, установленный во второй степени подвижности манипулятора, и первый квадратор, последовательно соединенные второй синусный функциональный преобразователь, вход которого подключен к выходу третьего датчика положения, и восьмой блок умножения, восьмой сумматор, выход которого соединен со вторым входом третьего блока умножения, последовательно соединенные девятый блок умножения, первый вход которого подключен к выходу третьего датчика скорости, десятый блок умножения и девятый сумматор, второй вход которого соединен с выходом одиннадцатого блока умножения, последовательно соединенные второй датчик ускорения, установленный во второй степени подвижности манипулятора, и двенадцатый блок умножения, последовательно соединенные третий задатчик постоянного сигнала и десятый сумматор, выход которого подключен ко второму входу первого блока умножения, второй вход - к выходу четвертого задатчика постоянного сигнала, а третий вход - ко второму входу седьмого блока умножения и к выходу одиннадцатого сумматора, первый и второй входы которого подключены к выходам пятого задатчика постоянного сигнала и датчика массы соответственно, последовательно соединенные тринадцатый блок умножения и двенадцатый сумматор, четырнадцатый блок умножения, первый вход которого подключен к выходу пятого задатчика постоянного сигнала, второй - к выходу четвертого сумматора, а выход - ко второму входу шестого сумматора, последовательно соединенные пятнадцатый блок умножения и шестнадцатый блок умножения, выход которого подключен к третьему входу девятого сумматора, последовательно соединенные первый дифференциатор, вход которого подключен к выходу первого датчика ускорения, и семнадцатый блок умножения, последовательно соединенные восемнадцатый блок умножения и девятнадцатый блок умножения, выход которого подключен ко второму входу двенадцатого сумматора, третий вход которого соединен с выходом двадцатого блока умножения, а также релейный элемент, вход которого подключен к выходу первого датчика скорости, а выход - к четвертому входу третьего сумматора, третий датчик ускорения, установленный на выходном валу редуктора, выход которого подключен к пятому входу третьего сумматора, шестой, седьмой и восьмой входы которого подключены, соответственно, к выходам седьмого, двадцать первого и двадцать второго блоков умножения, а первый вход последнего - к выходу девятого сумматора, второй дифференциатор, выход которого подключен к первому входу одиннадцатого блока умножения, последовательно соединенные четвертый датчик положения, третий косинусный функциональный преобразователь, выход которого соединен со вторым входом восьмого блока умножения, и двадцать третий блок умножения, второй вход которого подключен к выходу первого косинусного функционального преобразователя и входу пятнадцатого блока умножения, а выход - ко второму входу семнадцатого блока умножения, выход которого подключен к третьему входу седьмого сумматора, последовательно соединенные двадцать четвертый блок умножения, первый вход которого подключен к выходу одиннадцатого сумматора и второму входу четвертого блока умножения, второй вход - к выходу первого датчика ускорения, и двадцать пятый блок умножения, выход которого подключен к девятому входу третьего сумматора, а также двадцать шестой блок умножения, выход которого подключен к первому входу восьмого сумматора, второй вход которого соединен с выходом двадцать седьмого блока умножения, первый вход которого подключен к выходу четвертого датчика ускорения, четвертый синусный функциональный преобразователь, вход которого соединен с выходом четвертого датчика положения, а выход - с первым входом пятнадцатого блока умножения, второй вход которого подключен к выходу первого функционального преобразователя, и - с первым входом восемнадцатого блока умножения, второй вход которого подключен к выходу второго функционального преобразователя, а выход через двадцать восьмой блок умножения - к первому входу тринадцатого сумматора, второй вход которого подключен к выходу двадцать девятого блока умножения, последовательно соединенные тридцатый блок умножения и четырнадцатый сумматор, последовательно соединенные четвертый датчик скорости, второй квадратор, пятнадцатый сумматор, второй вход которого подключен к выходу первого квадратора, и тридцать первый блок умножения, второй вход которого подключен ко второму входу пятого блока умножения, к первому входу двадцать первого блока умножения, второй вход которого соединен с выходом четвертого блока умножения, и - к выходу четырнадцатого сумматора, в выход - к четвертому входу девятого сумматора, последовательно соединенные тридцать второй блок умножения, первый вход которого подключен к выходу пятнадцатого блока умножения, ко второму входу двадцать седьмого блока умножения и к первому входу тридцатого блока умножения, и шестнадцатый сумматор, выход которого подключен ко второму входу десятого блока умножения, а также тридцать третий блок умножения, первый вход которого подключен к выходу восьмого блока умножения, ко вторым входам одиннадцатого и двадцать шестого блоков умножения и к первому входу тридцать четвертого блока умножения, выход которого соединен со вторым входом шестнадцатого сумматора, а выход - ко второму входу четырнадцатого сумматора, тридцать пятый блок умножения, первый вход которого подключен к выходу девятнадцатого блока умножения, второй вход которого соединен с выходом девятого блока умножения, а выход - к десятому входу третьего сумматора, последовательно соединенные тридцать шестой блок умножения, семнадцатый сумматор и тридцать седьмой блок умножения, выход которого подключен к четвертому входу двенадцатого сумматора, пятый вход которого соединен с выходом восьмого сумматора, а выход - с первым входом шестого блока умножения, второй вход которого соединен с выходом второго датчика скорости, последовательно соединенные тридцать восьмой блок умножения, первый вход которого подключен к выходу шестого сумматора и вторым входам двадцать второго и тридцать пятого блоков умножения, а второй - к первому входу двадцатого блока умножения и к выходу пятнадцатого сумматора, и тридцать девятый блок умножения, выход которого соединен - с одиннадцатым входом третьего сумматора, последовательно соединенные сороковой блок умножения, восемнадцатый сумматор и сорок первый блок умножения, выход которого соединен с пятым входом девятого сумматора, последовательно соединенные сорок второй блок умножения, первый вход которого подключен к выходу второго датчика ускорения, входу второго дифференциатора, второму входу двадцать шестого и первому входу сорокового блоков умножения, девятнадцатый сумматор и сорок третий блок умножения, второй вход которого подключен к выходу двадцать третьего, вторым входам двадцатого, двадцать пятого и тридцать девятого и к первым входам двадцать девятого и тридцать шестого блоков умножения, а выход - к шестому входу девятого сумматора, седьмой вход которого соединен с выходом двенадцатого блока умножения, второй вход которого подключен к выходу семнадцатого сумматора, а восьмой вход - с выходом сорок четвертого блока умножения, первый вход которого подключен к выходу тринадцатого сумматора и к первому входу тринадцатого блока умножения, а второй вход - к выходу четвертого датчика ускорения, входу третьего дифференциатора, выход которого соединен со вторым входом шестнадцатого блока умножения, а также - к первым входам сорок пятого и сорок шестого блоков умножения, причем второй вход сорок пятого блока умножения подключен к выходу третьего датчика скорости и к вторым входам двадцать восьмого, тридцать второго, тридцать третьего, тридцать шестого, тридцать седьмого и сорок второго блоков умножения, а выход - к второму входу восемнадцатого сумматора, второй вход сорок шестого блока умножения подключен к выходу четвертого датчика скорости, к вторым входам девятого, тринадцатого, двадцать девятого, тридцатого, тридцать четвертого, сорокового и - к первому входу сорок седьмого блоков умножения, а выход -к второму входу девятнадцатого сумматора, второй вход сорок седьмого блока умножения соединен с выходом восемнадцатого блока умножения и вторым входом сорок первого блока умножения, а выход - со вторым входом семнадцатого сумматора (Патент РФ №2488008, Бюл. №19, 2013).

Данное устройство по своей технической сущности является наиболее близким к предлагаемому изобретению.

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

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

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

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

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

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

На фиг.1 представлена блок-схема предлагаемого электропривода манипулятора, а на фиг.2 - кинематическая схема манипулятора.

Электропривод манипулятора содержит последовательно Электропривод манипулятора, содержащий последовательно соединенные первый 1 и второй 2 сумматоры, первый блок 3 умножения, третий сумматор 4, усилитель 5, электродвигатель 6, связанный непосредственно с первым датчиком 7 скорости, выход которого подключен ко вторым входам второго 2 и третьего 4 сумматоров, и через редуктор 8 - с первым датчиком 9 положения, выход которого подключен к первому входу первого сумматора 1, второй вход которого соединен со входом устройства, последовательно соединенные второй датчик 10 положения, установленный в третьей степени подвижности манипулятора, четвертый сумматор 11, второй вход которого подключен к выходу первого задатчика 12 постоянного сигнала, пятый сумматор 13, второй вход которого подключен к выходу второго задатчика 14 постоянного сигнала, второй блок 15 умножения, второй вход которого подключен к выходу датчика 16 массы, шестой сумматор 17 и третий блок 18 умножения, выход которого подключен к третьему входу третьего сумматора 4, последовательно соединенные третий датчик 19 положения, установленный во второй степени подвижности манипулятора, и первый косинусный функциональный преобразователь 20, последовательно соединенные второй датчик 21 скорости, установленный в третьей степени подвижности манипулятора, и четвертый блок 22 умножения, последовательно соединенные первый датчик 23 ускорения, установленный в третьей степени подвижности манипулятора, пятый блок 24 умножения, седьмой сумматор 25, второй вход которого подключен к выходу шестого блока 26 умножения и седьмой блок 27 умножения, последовательно соединенные третий датчик 28 скорости, установленный во второй степени подвижности манипулятора, и первый квадратор 29, последовательно соединенные второй синусный функциональный преобразователь 30, вход которого подключен к выходу третьего датчика 19 положения, и восьмой блок 31 умножения, восьмой сумматор 32, выход которого соединен со вторым входом третьего блока 18 умножения, последовательно соединенные девятый блок 33 умножения, первый вход которого подключен к выходу третьего датчика 28 скорости, десятый блок 34 умножения и девятый сумматор 35, второй вход которого соединен с выходом одиннадцатого блока 36 умножения, последовательно соединенные второй датчик 37 ускорения, установленный во второй степени подвижности манипулятора, и двенадцатый блок 38 умножения, последовательно соединенные третий задатчик 39 постоянного сигнала и десятый сумматор 40, выход которого подключен ко второму входу первого блока 3 умножения, второй вход - к выходу четвертого задатчика 41 постоянного сигнала, а третий вход - ко второму входу седьмого блока 27 умножения и к выходу одиннадцатого сумматора 42, первый и второй входы которого подключены к выходам пятого задатчика 43 постоянного сигнала и датчика 16 массы, соответственно, последовательно соединенные тринадцатый блок 44 умножения и двенадцатый сумматор 45, четырнадцатый блок 46 умножения, первый вход которого подключен к выходу пятого задатчика 43 постоянного сигнала, второй - к выходу четвертого сумматора 11, а выход -ко второму входу шестого сумматора 17, пятнадцатый блок 47 умножения, шестнадцатый блок 48 умножения, выход которого подключен к третьему входу девятого сумматора 35, последовательно соединенные первый дифференциатор 49, вход которого подключен к выходу первого датчика 23 ускорения, и семнадцатый блок 50 умножения, выход которого подключен к третьему входу седьмого сумматора 25, восемнадцатый блок 51 умножения, девятнадцатый блок 52 умножения, выход которого подключен ко второму входу двенадцатого сумматора 45, третий вход которого соединен с выходом двадцатого блока 53 умножения, а также релейный элемент 54, вход которого подключен к выходу первого датчика 7 скорости, в выход - к четвертому входу третьего сумматора 4, третий датчик 55 ускорения, установленный на выходном валу редуктора 8, выход которого подключен к пятому входу третьего сумматора 4, шестой, седьмой и восьмой входы которого подключены, соответственно, к выходам седьмого 27, двадцать первого 56 и двадцать второго 57 блоков умножения, а первый вход последнего - к выходу девятого сумматора 35, второй дифференциатор 58, выход которого подключен к первому входу одиннадцатого блока 36 умножения, последовательно соединенные четвертый датчик 59 положения, третий косинусный функциональный преобразователь 60, выход которого соединен со вторым входом восьмого блока 31 умножения, и двадцать третий блок 61 умножения, второй вход которого подключен к выходу первого функционального преобразователя 20 и к первому входу пятнадцатого блока 47 умножения, последовательно соединенные двадцать четвертый блок 62 умножения, первый вход которого подключен к выходу одиннадцатого сумматора 42 и второму входу четвертого блока 22 умножения, а второй вход - к выходу первого датчика 23 ускорения, и двадцать пятый блок 63 умножения, выход которого подключен к девятому входу третьего сумматора 4, а также двадцать шестой блок 64 умножения, выход которого подключен к первому входу восьмого сумматора 32, второй вход которого соединен с выходом двадцать седьмого блока 65 умножения, первый вход которого подключен к выходу четвертого датчика 66 ускорения, четвертый синусный функциональный преобразователь 67, вход которого соединен с выходом четвертого датчика 59 положения, а выход - с первым входом пятнадцатого блока 47 умножения, и - с первым входом восемнадцатого блока 51 умножения, второй вход которого подключен к выходу второго функционального преобразователя 30, последовательно соединенные двадцать восьмой блок 68 умножения и тринадцатый сумматора 69, второй вход которого подключен к выходу двадцать девятого блока 70 умножения, последовательно соединенные тридцатый блок 71 умножения, и четырнадцатый сумматор 72, последовательно соединенные четвертый датчик 73 скорости, второй квадратор 74, пятнадцатый сумматор 75, второй вход которого подключен к выходу первого квадратора 29, и тридцать первый блок 76 умножения, второй вход которого подключен ко второму входу пятого блока 24 умножения, к первому входу двадцать первого блока 56 умножения, второй вход которого соединен с выходом четвертого блока 22 умножения, и - к выходу четырнадцатого сумматора 72, в выход - к четвертому входу девятого сумматора 35, последовательно соединенные тридцать второй блок 77 умножения, первый вход которого подключен к первому входу шестнадцатого блока 48 умножения, ко второму входу двадцать седьмого блока 65 умножения, к первому входу тридцатого блока 71 умножения, и шестнадцатый сумматор 78, выход которого подключен ко второму входу десятого блока 34 умножения, а также тридцать третий блок 79 умножения, первый вход которого подключен ко вторым входам одиннадцатого 36 и двадцать шестого 64 блоков умножения, а также - к первому входу тридцать четвертого блока 80 умножения, выход которого соединен со вторым входом шестнадцатого сумматора 78, а выход - ко второму входу четырнадцатого сумматора 72, тридцать пятый блок 81 умножения, первый вход которого подключен к выходу девятнадцатого блока 52 умножения, второй вход которого соединен с выходом девятого блока 33 умножения, а выход - к десятому входу третьего сумматора 4, последовательно соединенные тридцать шестой блок 82 умножения, семнадцатый сумматор 83 и тридцать седьмой блок 84 умножения, выход которого подключен к четвертому входу двенадцатого сумматора 45, пятый вход которого соединен с выходом восьмого сумматора 32, а выход - с первым входом шестого блока 26 умножения, второй вход которого соединен с выходом второго датчика 21 скорости, последовательно соединенные тридцать восьмой блок 85 умножения, первый вход которого подключен к выходу шестого сумматора 17 и вторым входам двадцать второго 57 и тридцать пятого блоков 81 умножения, а второй - к первому входу двадцатого блока 53 умножения и к выходу пятнадцатого сумматора 75, и тридцать девятый блок 86 умножения, выход которого соединен - с одиннадцатым входом третьего сумматора 4, последовательно соединенные сороковой блок 87 умножения, восемнадцатый сумматор 88 и сорок первый блок 89 умножения, выход которого соединен с пятым входом девятого сумматора 35, последовательно соединенные сорок второй блок 90 умножения, первый вход которого подключен к выходу второго датчика 37 ускорения, входу второго дифференциатора 58, к первым входам двадцать шестого 64 и сорокового 87 блоков умножения, девятнадцатый сумматор 91 и сорок третий блок 92 умножения, второй вход которого подключен ко вторым входам семнадцатого 50, двадцатого 53, двадцать пятого 63 блоков умножения, и к первым входам двадцать девятого 70, тридцать шестого 82 и тридцать девятого 86 блоков умножения, а выход - к шестому входу девятого сумматора 35, седьмой вход которого соединен с выходом двенадцатого блока 38 умножения, второй вход которого подключен к выходу семнадцатого сумматора 83, а восьмой вход - с выходом сорок четвертого блока 93 умножения, первый вход которого подключен к выходу тринадцатого сумматора 69 и к первому входу тринадцатого блока 44 умножения, а второй - к выходу четвертого датчика 66 ускорения, входу третьего дифференциатора 94, выход которого соединен со вторым входом шестнадцатого блока 48 умножения, а также - к первым входам сорок пятого 95 и сорок шестого 96 блоков умножения, причем второй вход сорок пятого блока 95 умножения подключен к выходу третьего датчика 28 скорости и к вторым входам двадцать восьмого 68, тридцать второго 77, тридцать третьего 79, тридцать шестого 82, тридцать седьмого 84 и сорок второго 90 блоков умножения, а выход - к второму входу восемнадцатого сумматора 88, второй вход сорок шестого блока 96 умножения подключен к выходу четвертого датчика 73 скорости, к вторым входам девятого 33, тринадцатого 44, двадцать девятого 70, тридцатого 71, тридцать четвертого 80, сорокового 87 и - к первому входу сорок седьмого 97 блоков умножения, а выход - к второму входу девятнадцатого сумматора 91, второй вход сорок седьмого блока 97 умножения соединен с первым входом девятнадцатого блока 52 умножения, со вторыми входами двадцать восьмого 68 и сорок первого блоков 89 умножения, а выход - со вторым входом семнадцатого сумматора 83, а также шестерню 98. При этом выходы восьмого 31, пятнадцатого 47, восемнадцатого 51 и двадцать третьего 61 блоков умножения соединяются, соответственно, с первым входом девятнадцатого 52 и со вторыми входами семнадцатого 50, двадцать шестого 64 и двадцать седьмого 65 блоков умножения.

На чертежах введены следующие обозначения: q_вх - сигнал желаемого положения шестой степени подвижности манипулятора; q_i - соответствующие обобщенные координаты манипулятора; - скорости изменения соответствующих обобщенных координат; ε - ошибка электропривода (величина рассогласования); m_l,m₂,m_г - соответственно, массы первого, второго, звеньев манипулятора и захваченного груза; l₁=const - длина первого звена манипулятора; исходное расстояние от оси вращения второго звена до его центра масс при q₃=0; l₂=const - расстояние от центра масс второго звена до средней точки схвата; - скорость вращения ротора электродвигателя пятой степени подвижности; α_p5, - соответственно, угол поворота выходного вала редуктора рассматриваемого электропривода с передаточным отношением i_p и ускорение вращения этого вала; U^*,U - соответственно усиливаемый сигнал и сигнал управления электродвигателем 6.

Рассматриваемый электропривод управляет обобщенной координатой q₅. Конструкция манипулятора (фиг.2) позволяет осуществлять горизонтальное перемещение груза (координата q₄,q₅) в любых направлениях и вращение вокруг вертикальной и горизонтальной осей (координаты q₁,q₂, соответственно), а также телескопическое перемещение первого и второго звеньев (координата q₃,q₆).

Устройство работает следующим образом. Сигнал ошибки £ после коррекции в блоках 1, 2, 3, 4, усиливаясь, поступает на электродвигатель 6, приводя его вал во вращательное движение с направлением и скоростью (ускорением), зависящими от величины поступающего сигнала U, моментов трения и внешнего моментного воздействия M_B. Электропривод при работе манипулятора с различными грузами, а также за счет взаимовлияния между всеми его степенями подвижности обладает переменными моментными характеристиками, которые могут меняться в широких пределах. Это снижает качественные показатели работы электропривода и даже приводит к потере устойчивости его работы. В результате возникает задача, связанная с обеспечение инвариантности динамических свойств этого электропривода к непрерывным и быстрым изменениям его моментных нагрузочных характеристик, что позволяет обеспечить его точную и устойчивую работу.

Моментные характеристики электропривода, управляющего координатой q₅, зависят от изменения координат q₁,q₂,q₃,,,,,,,,,, и груза m_Г. В связи с этим для качественного управления координатой q₅ необходимо точно компенсировать отрицательное влияние изменения этих координат, а также переменной массы груза m_Г на динамические свойства рассматриваемого электропривода (координата q₅).

Манипулятор в горизонтальной плоскости перемещается с помощью рассматриваемого электропривода посредством передачи шестерня-рейка (координаты q₄, q₅). Причем рейка установлена в основании манипулятора, а шестерня 98 радиусом r - на выходном валу редуктора 8 электропривода.

С помощью уравнений Лагранжа второго рода несложно показать, что в процессе движения манипулятора на его линейную горизонтальную обобщенную координату q₅ действует сила

которая в процессе этого движения создает на выходном валу редуктора 8 момент, равный

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

где R и L - активное и индуктивное сопротивления якорной цепи электродвигателя 6, соответственно; K_M - коэффициент крутящего момента; J - момент инерции якоря электродвигателя и вращающихся частей редуктора, приведенный к валу электродвигателя; k_ω - коэффициент противо-ЭДС; k_B - коэффициент вязкого трения; М_стр - момент сухого трения; k_у - коэффициент усиления усилителя 5; i - ток якоря электродвигателя 6; , - соответственно, третья производная и ускорение вращения вала электродвигателя 6; ;

Из выражения (2) видно, что параметры этого уравнения, а, следовательно, параметры и динамические свойства электропривода, управляющего координатой q₅, являются существенно переменными, зависящими от q₁,q₂,q₃,,,,,,,,, и m_Г. В результате для реализации поставленной выше задачи необходимо сформировать такое корректирующее устройство, которое застабилизировало бы параметры этого электропривода таким образом, чтобы он описывался дифференциальным уравнением с постоянными желаемыми параметрами.

Первый отрицательный вход сумматора 1 (со стороны датчика 9) имеет коэффициент усиления, равный r, а его второй вход - единичный коэффициент усиления. В результате на выходе этого сумматора формируется сигнал ε=q_вх-α_p5r.

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

Датчики 59, 19 и 10 установлены, соответственно, в первой, второй и третьей степенях подвижности манипулятора и измеряют обобщенные координаты q₁, q₂ и q₃, соответственно. Датчики 73, 28 и 21 установлены, соответственно, в его первой, второй и третьей степенях подвижности и измеряют скорости , и , а датчики 66, 37 и 23 - в его первой, второй и третьей степенях подвижности и измеряют ускорения , и соответственно.

Задатчики 12 и 14 формируют сигналы и l₂, соответственно, а задатчики 39, 41 и 43 - сигналы , m₁ и m₂, соответственно. Датчик 16 измеряет m_Г.

На выходе сумматора 42, имеющего положительные входы с единичными коэффициентами усиления, формируется сигнал m₂+m_Г, на выходе блока 62 - сигнал , на выходе блока 47 - сигнал sinq₁cosq₂, а на выходе блока 63 - сигнал . Первый положительный вход сумматора 40 (со стороны блока 39) имеет единичный коэффициент усиления, второй (со стороны блока 41) и третий положительные входы - коэффициенты усиления r². В результате на выходе сумматора 40 формируется сигнал

На выходе сумматора 11, положительные входы которого имеют единичные коэффициенты усиления, формируется сигнал , на выходе блока 46 - сигнал , на выходе сумматора 13, положительные входы которого имеют единичные коэффициенты усиления, - сигнал , на выходе блока 15 - сигнал , а на выходе сумматора 17, положительные входы которого имеют единичные коэффициенты усиления, - сигнал .

На выходе блока 61 формируется сигнал cosq₁cosq₂, на выходе блока 65 - сигнал , на выходе блока 51 - сигнал sinq₁sinq₂, на выходе блока 64 - сигнал , на выходе сумматора 32, первый положительный (со стороны блока 64) и второй отрицательный входы которого имеют единичные коэффициенты усиления, - сигнал , а на выходе блока 18 - сигнал .

На выходе блоков 71 и 79 формируются сигналы и , соответственно, на выходе сумматора 72, первый положительный (со стороны блока 71) и второй отрицательный входы которого имеют единичные коэффициенты усиления, - сигнал , на выходе блока 22 - сигнал , а на выходе блока 56 - сигнал .

На выходе сумматора 75, положительные входы которого имеют единичные коэффициенты усиления, формируется сигнал , на выходе блока 85 - сигнал , а на выходе блока 86 - сигнал .

На выходе блока 33 формируется сигнал , на выходе блока 31 - сигнал cosq₁sinq₂, на выходе блока 52 - сигнал , а на выходе блока 81 появляется сигнал .

На выходе блока 50 формируется сигнал , на выходе блока 24 - сигнал , а на выходе блока 53 - сигнал .

На выходе блока 70 формируется сигнал , а на выходе блока 68 - сигнал . В результате на выходе сумматора 69, положительные входы которого имеют единичные коэффициенты усиления, формируется сигнал , а на выходе блока 44 - сигнал .

На выходе блока 82 формируется сигнал , на выходе блока 97 - сигнал , в результате на выходе сумматора 83, положительные входы которого имеют единичные коэффициенты усиления, формируется сигнал , а на выходе блока 84 - сигнал .

Первый (со стороны блока 44), второй (со стороны блока 52) и четвертый (со стороны блока 84) положительные входы сумматора 45 имеют коэффициенты усиления, равные 2, пятый положительный вход (со стороны сумматора 32) - коэффициент усиления, равный 3, а третий положительный (со стороны блока 53) - единичный коэффициент усиления. В результате на выходе блока 26 формируется сигнал

.

Первый отрицательный вход сумматора 25 (со стороны блока 24) имеет коэффициент усиления, равный 3, а его второй положительный (со стороны блока 26) и третий отрицательный входы - единичные коэффициенты усиления. В результате на выходе блока 27 формируется сигнал

На выходе блока 93 формируется сигнал на выходе блока 76 - сигнал , на выходе блока 36 - сигнал , на выходе блока 48 - сигнал , на выходе блока 38 - сигнал , на выходе блока 96 - сигнал , а на выходе блока 90 - сигнал , на выходе сумматора 91, положительные входы которого имеют единичные коэффициенты усиления, - сигнал , на выходе блока 92 - сигнал , на выходе блока 87 - сигнал , а на выходе блока 95 - сигнал , на выходе сумматора 88, положительные входы которого имеют единичные коэффициенты усиления, - сигнал , на выходе блока 89 - сигнал , на выходе блока 77 - сигнал , на выходе блока 80 - сигнал , на выходе сумматора 78, отрицательный (со стороны блока 80) и положительный входы которого имеют единичные коэффициенты усиления, - сигнал , а на выходе блока 34 - сигнал .

Первый (со стороны блока 34), пятый (со стороны блока 89) и шестой (со стороны блока 92) положительные входы сумматора 35 имеют коэффициенты усиления, равные 2, а его третий (со стороны блока 48) отрицательный вход и все остальные положительные входы имеют единичные коэффициенты усиления. В результате на выходе блока 57 формируется сигнал

Датчик 55 измеряет ускорение . Выходной сигнал релейного элемента 54 имеет вид

где |M_T| - величина момента сухого трения при движении.

Первый (со стороны блока 3), второй (со стороны датчика 7), четвертый (со стороны релейного элемента 54), пятый (со стороны датчика 55), шестой (со стороны блока 27), восьмой (со стороны блока 57) положительные входы сумматора 4, соответственно, имеют коэффициенты усиления , , , , , , где J_H - номинальное (желаемое) значение приведенного момента инерции электропривода. Десятый положительный (со стороны блока 81) и седьмой отрицательный (со стороны блока 56) входы этого сумматора имеют коэффициенты усиления , а девятый (со стороны блока 63) отрицательный, третий (со стороны блока 18) и одиннадцатый (со стороны блока 86) положительные входы - коэффициенты усиления . В результате на выходе сумматора 4 формируется сигнал

Несложно показать, что, поскольку при движении электропривода достаточно точно соответствует M_стр, то, подставив полученное значение U^* (3) в соотношение (2), получим уравнение , которое имеет постоянные желаемые параметры. То есть предложенный электропривод, управляющий координатой q₅, будет обладать постоянными желаемыми динамическими свойствами и качественными показателями.

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

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

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