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

При первоначальной настройке и обслуживании приводной системы манипулятора УДР, операции хоуминга выполняются в следующей последовательности:
  1. Хоуминг горизонтального привода.
  2. Хоуминг вертикального привода.
  3. Коррекция поворотного привода.

1.1.1 Хоуминг вертикального привода (по оси Z)
Перед проведением операции хоуминга необходимо убедиться в корректном расположении упоров. После отправки команды для выполнения хоуминга осуществляется «медленное» перемещение манипулятора вниз до соприкосновения с поверхностью пола. При достижении упоров происходит резкое возрастание момента сопротивления движению манипулятора и, как следствие, бросок тока в обмотках электродвигателя. Серворегулятор привода регистрирует бросок тока и отключает перемещение, после чего записывает в текущее значение положения нулевую координату. Для режима хоуминга вертикального привода возможно настроить ряд параметров:
  1. Направление перемещения.
  2. Скорость перемещения.
  3. Ускорение разгона и торможения.
  4. Регистрируемый момент (в % от максимального) триггера.
  5. Время фиксации нулевого положения после срабатывания триггера.
  6. Смещение нулевого положения.

1.1.2 Хоуминг горизонтального привода (по оси Y)
После отправки команды для выполнения хоуминга осуществляется «медленное» перемещение манипулятора влево (в сторону кормушки). При достижении упоров происходит резкое возрастание момента сопротивления движению манипулятора и, как следствие, бросок тока в обмотках электродвигателя. Серворегулятор привода регистрирует бросок тока и отключает перемещение, после чего записывает в текущее значение положения нулевую координату. Для режима хоуминга горизонтального привода возможно настроить ряд параметров:
  1. Направление перемещения.
  2. Скорость перемещения.
  3. Ускорение разгона и торможения.
  4. Регистрируемый момент (в % от максимального) триггера.
  5. Время фиксации нулевого положения после срабатывания триггера.
  6. Смещение нулевого положения.

1.1.3 Корректировка поворотного привода (ось X)
Установка центрального положения (корректировка поворотного привода) осуществляется с использованием специального центрирующего устройства фиксируемого на УДР. Корректировка поворотного привода выполняется только после проведения операции хоуминга двух других приводов. С использованием ручного перемещения осуществляется перемещение манипулятора в центральное положение, определяемое специальным устройством. После корректной установки в центральном положении осуществляется фиксация нулевого положения для поворотного привода, данные записываются в память сервоконтроллера.

1.2 Ручное управление манипулятором
Ручное управление манипулятором осуществляется через интерфейс. Перемещение манипулятора осуществляется нажатием соответствующей клавиши. Ручное перемещение может осуществляться в «быстром» и «медленном» режиме. Переключении режимов осуществляется нажатием. При «быстром» перемещении перемещение производится только в том случае когда кнопка зажата. При этом скорость плавно увеличивается до максимально допустимой для данного режима перемещения. После отпускания кнопки перемещения, осуществляется плавный останов. В случае «медленного» режима, перемещение осуществляется дискретным нажатием кнопки. При нажатии кнопки перемещение осуществляется на заранее зафиксированное минимальное смещение.
Перечень алгоритмов управления
01
1.3 Автоматическое управление манипулятором
1.3.1 Перемещение в стандартные положения

Перемещение в заданные стандартные положения осуществляется при задании управления от соответствующего алгоритма. Стандартные положения фиксируются при начальной настройке системы позиционирования. Некоторые параметры стандартных положений могут быть скорректированы вручную, а некоторые положения задаются программно при корректировке. Для системы позиционирования манипулятора заданы следующие стандартные положения манипулятора в пространстве:
  1. Центральное положение в доильном боксе;
  2. Положение ополаскивания;
  3. Положение промывки;
  4. Положение обслуживания;
  5. Положение ожидания.
1.3.2 Перемещения при доении
Во время процедуры доения осуществляется автоматическое перемещение манипулятора в требуемую координату. Перемещения манипулятора основаны на компенсировании смещения животного в доильном боксе, на основе данных получаемых с камеры слежения за перемещением животного в боксе. Перемещения при доении включает следующие алгоритмы:
  1. Алгоритм слежения за перемещением животного при доении
  2. Перемещение при подключении доильных стаканов
  3. Перемещение при отключении доильных стаканов
Рисунок 1.1. Интерфейс ручного управления
Рисунок 1.2. Переключение режимов (а - быстрое, б - медленное)
2.1 Модель кинематики
На рисунке 2.1 представлено окно задания основных параметров имитационной модели системы позиционирования.

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

На рисунке 2.3 представлено окно динамики системы позиционирования манипулятора. Предназначено для задания параметров исполнительной системы и выполнение построения траекторий движения.
Имитационное моделирование манипулятора
02
Рисунок 2.1. Параметры модели
Рисунок 2.2. Модель кинематики
Рисунок 2.3. Модель динамики
2.2 Отладочная модель
Отладочная модель позволяет осуществлять непосредственное управления исполнительными механизмами и выполнять отработку реальных алгоритмов для применения на конечном продукте.

Окно управления приводом (рисунок 2.4) обладает следующим функционалам:
  • Ведения журнала (логгирования) команд по интерфейсу управления RS-485.
  • Включение/отключение коммуникации RS-485 и регулятора.
  • Опрос датчика положения (однократный и многократный с частотой 50 мс).
  • Операция хоуминга по броску момента при движении (настраиваемая амплитуда).
  • Движение на один оборот по/против часовой стрелки.
  • Непрерывное вращение в одном направлении.
  • Прямое управление скоростью привода.
  • Регулятор координат (ПИД-регулятор) с возможностью настройки параметров.
Рисунок 2.4. Окно управления приводом
Окно управления приводом (рисунок 2.4) обладает следующим функционалам:
  • Ведения журнала (логгирования) команд по интерфейсу управления RS-485.
  • Включение/отключение коммуникации RS-485 и регулятора.
  • Опрос датчика положения (однократный и многократный с частотой 50 мс).
  • Операция хоуминга по броску момента при движении (настраиваемая амплитуда).
  • Движение на один оборот по/против часовой стрелки.
  • Непрерывное вращение в одном направлении.
  • Прямое управление скоростью привода.
  • Регулятор координат (ПИД-регулятор) с возможностью настройки параметров.
Окно решения кинематических задач (рисунок 2.5) обладает следующим функционалам:
  • Решения прямой задачи кинематики о положении манипулятора УДР и построение этого решения на схеме.
  • Решения обратной задачи кинематики (аналитически и численно) о положении манипулятора УДР и построение этого решения на схеме.
  • Выбор и задание конечной точки (рабочего органа) манипулятора УДР.
  • Перемещение рабочего органа манипулятора УДР по с заданной скоростью по фиксированным направлениям (поворот влево/вправо, перемещение по оси Y влево/вправо, перемещение по оси Х вверх/вниз плоскости манипулятора).
  • Хоуминг привода.
  • Связь с оборудованием: построения графика на основе данных получаемых с двигателя (частота опроса положения 50 мс).
Рисунок 2.5. Окно решения кинематических задач
3.1 Описание режима позиционного управления
Режим позиционного управления заключается в задании необходимого смещения привода. В таблицу положений записывается координата с указанием следующих параметров:
  • положение в импульсах;
  • скорость вращения в об/мин;
  • время ускорения от 0 до 1000 об/мин в мс;
  • время торможения от 1000 об/мин до 0 в мс;
  • продолжительность задержки между командами;
  • метод задания положения (относительный или абсолютный);
  • количество пульсов сигнализирующее успешное выполнение перемещения;
  • активность команды (включена/отключена).
Практическая реализация управления приводом
03
Рисунок 2.5. Таблица настройки позиции
3.2 Описание режима управления скоростью
При формировании управления по скорости, для реализации позиционирования, необходимо обеспечить функционирование внешнего (по отношению к сервоконтроллеру) регулятора координат.
В общем виде алгоритм режима управления по скорости включает следующие шаги:
  1. Задание предустановленной скорости привода равное 0 об/мин.
  2. Включение регулятора сервоконтроллера.
  3. Включение задания скорости от первого предустановленного значения.
  4. Получение текущего положения привода.
  5. Расчет смещения относительно задания.
  6. Расчет управляющего задания скорости используя ПИД-регулятор.
  7. Задание предустановленной скорости привода равное рассчитанному значению.
  8. Повтор с шага 4 до тех пор, пока смещение больше допустимого значения (допустимая ошибка позиционирования).
  9. Отключение задания скорости.
  10. Позиционирование выполнено успешно.
4.1 Технические характеристики и назначение ПО
Назначение ПО: тестирование и отладка программы управления УДР, формирование, отправка и прием команд управления для УДР и его подсистем.
Технические характеристики:
  • Целевая ОС: Windows 7.0;
  • Целевой framework: .net 6.0;
  • Минимальное разрешение: 1920х1080;
  • Среда: Windows Presentation Foundation (WPF).

4.2 Окно настройки соединения (Связь)
Данный интерфейс обладает следующим функционалом:
  • Выбор и подключение к ПО через интерфейсы: UDP, TCP, SignalR, UnixSocket.
  • Задание адреса устройства управления УДР.
  • Авто-подержание соединения.
  • Вывод общего журнала сообщений.

4.3 Окно тестирования и формирования команд управления (Команды)
Данный интерфейс обладает следующим функционалом:
  • Выбор управляющей программы (или подсистемы УДР) и выбор команды управления из списка доступных команд.
  • Формирование и отправка команды с заданными параметрами.
  • Циклическая отправка команды управления.
  • Вывод общего журнала сообщений и сообщений отправленных команд.
Описание программного обеспечения
04
Рис.41. Настройки соединения
4.4 Окно управления движением манипулятора (Движение)
Данный интерфейс обладает следующим функционалом:
  • Включение/отключение питания двигателей приводной системы манипулятора УДР.
  • Ручное перемещение манипулятора по всем степеням подвижности.
  • Задание скорости перемещения.
  • Сброс ошибок подсистемы управления движением (сервоконтроллера приводной системы).
  • Абсолютное/относительно перемещение рабочего органа (лазер, доильный стакан 1, 2, 3 и 4) манипулятора в заданную точку.
  • Получение текущих координат рабочего органа манипулятора.
  • Перемещение манипулятора в стандартные положения.
Рис.42. Тестирования и формирования команд управления
Рис.4.3. Управление движением манипулятора
4.5 Окно управления сигналами программируемого логического контроллера (ПЛК)
Данный интерфейс обладает следующим функционалом:
  • Перевод УДР и ПЛК в режим «Старт», «Стоп» и «Пауза».
  • Выбор и задание состояния сигнала ПЛК (включен/отключен) согласно списку в главе 2.
Рис.4.4. Управление сигналами ПЛК
4.6 Окно обмена данными с системой Умная Ферма 4.0 (АРМ)
Данный интерфейс обладает следующим функционалом:
  • Тестирование подключения к внешнему серверу данных через SignalR (УФ 4.0).
  • Отправка команд внешнему серверу данных через SignalR (УФ 4.0).
  • Формирование случайного сообщения для УФ 4.0.
  • Эмуляция внешнего сервера локально с заданным адресом.
  • Обмен данными с имитацией внешнего сервера.
Рис.4.5. Обмен данными с системой Умная Ферма 4.0
4.7 Окно настроек ПО (Настройки)
Данный интерфейс обладает следующим функционалом:
  • Настройки цветного отображения (выбор цвета рис.7) списка команд.
  • Настройки цветного отображения (выбор цвета рис.7) общего списка сообщений.
Рис.4.6. Настройки ПО
Рис.4.7. Выбор цвета
5.1 Структура ПО для управления УДР
Структура ПО и основные команды
05
Рис. 5.1. Общая структура ПО
Рис. 5.2. Структура подпрограмм УДР
Рис. 5.3. Структура объектов состояния (параметров) УДР
Рис. 5.4. Структура утилит ПО (загрузка/сохранение данных)
Рис. 5.5. Структура подпрограммы управления приводной системой манипулятора УДР
Рис. 5.6. Структура подпрограммы управления ПЛК
Рис. 5.7. Структура подпрограммы для взаимодействия с ЧМИ
Технологии
Для обеспечения кинематики доильного робота
  • C# и C++: Языки программирования, используемые для написания высокопроизводительного кода.
  • · .NET 8.0 для поддержки современных библиотек и API.
  • · PostgreSQL и SQLite для управления данными.
  • · SignalR, WebSockets, и UnixSockets для мгновенной коммуникации.
  • · ModBus TCP и EtherCAT для интеграции с промышленным оборудованием.