SWD Software Ltd. - официальный дистрибьютор QNX на территории России и стран бывшего СССР Операционная система реального времени QNX
Инструменты для создания надёжных встраиваемых систем и
интеллектуальных устройств любой сложности
QNX Software Systems - разработчик встраиваемой операционной системы QNX
  Стандартная версия

Принципы построения программного обеспечения системы управления антропоморфным шагающим роботом

Principles of the control system software building for anthropomorphic steptraveling robot

Ковальчук А.К., Кулаков Д.Б., Семёнов С.Е.

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

В МГТУ им. Н.Э.Баумана усилиями сотрудников двух подразделений - Межотраслевого института повышения квалификации и кафедры Гидромеханики, гидромашин и гидропневмоавтоматики - разрабатывается антропоморфный шагающий робот. Для управления роботом в реальном времени приходится решать много разноплановых задач, относящихся к компетенции различных специалистов (например, планирование траектории движения к месту назначения, размещение на опорной поверхности следовых точек, формирование траекторий движения отдельных звеньев исполнительного механизма робота на каждом шаге). В процессе движения необходимо решать обратную задачу кинематики робота и управлять работой его системы приводов, постоянно контролировать состояние робота, опрашивать множество датчиков и формировать управляющие воздействия на исполнительные системы робота.

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

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

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

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

Эти задачи требуют периодического выполнения определенных алгоритмов, причем для первой группы типична частота повторения порядка 10 Гц, для второй - 100 Гц, а для третьей - 1000 Гц. К третьей группе приходится и основная доля взаимодействия с различным внешним по отношению к системе управления оборудованием.

Очевидно, что система управления антропоморфным шагающим роботом, призванная решать разнообразные и сложные задачи, должна строиться на основе сети компьютеров с соответствующим программным обеспечением системы управления (ПОСУ).

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

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

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

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

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

Кроме того, в QNX имеется хотя и не POSIX-совместимое, но очень эффективное в системах жесткого реального времени средство взаимодействия процессов – сообщение. Различают блокирующие (обеспечиваются функциями ОС MsgSend(), MsgReceive(), MsgReply() и др.) и неблокирующие (обеспечиваются функциями MsgReceivePulse(), MsgSendPulse()) сообщения [1]. Они позволяют с минимальными накладными расходами синхронизировать выполнение процессов и обеспечивают передачу данных.

Одно из важнейших свойств QNX – малое и гарантированно не превышающее определенный предел время реакции на прерывание. Именно это свойство делает данную ОС системой жесткого реального времени.

В ОС QNX каждый процесс выполняется в своем, не доступном другим процессам, адресном пространстве и имеет назначаемый пользователем приоритет, который может превышать приоритет системных процессов. Оформление каждой задачи или группы задач в виде отдельного процесса позволяет уменьшить степень неконтролируемого влияния решаемых задач управления друг на друга. Программировать такие ПЗ могут разные специалисты по решению конкретной задачи. При этом они меньше мешают друг другу. Ранее же, если новый разработчик включал участок своего кода в программу, разработанную другим человеком, даже под его контролем, часто возникали трудно выявляемые ошибки. Иногда ПОСУ полностью разрушалось, и большой объем работы приходилось делать заново.

Средства QNX позволяют одновременно выполняющимся процессам обмениваться сообщениями без посредников. Если система не очень сложная, такой подход позволяет избежать накладных расходов. Однако, по мере усложнения системы и увеличения срока работы над ней, непосредственные связи становятся запутанными. Если учесть, что ПОСУ робота – это динамическая система, такая структура может работать неустойчиво и часто возникают сбои.

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

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

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

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

В качестве основного структурообразующего принципа построения ПОСУ принята схема клиент-сервер. Сервером служит многопоточный ПТ. ПЗ являются клиентами. Для взаимодействия с каждым ПЗ в процессе инициализации системы ПТ создает по два отдельных потока-сервера; они, в свою очередь, - нумерованные каналы для связи ПЗ с ПТ. Одна из пар поток-канал предназначена для обеспечения обмена данными, другая - для синхронизации во времени работы ПЗ между собой.

Основным средством синхронизации работы отдельных ПЗ между собой и привязки их к реальному времени является событие. Термин событие специфический для данного ПОСУ. В каком-то смысле, событие – это аналог программного прерывания. Каждое событие имеет уникальный в рамках ПОСУ номер. Номера событиям могут присваиваться произвольно. Единственное условие – номера должны идти по порядку. Сейчас максимальное значение номера события - 256, но при необходимости оно быть увеличено.

Каждый ПЗ может генерировать событие с любым номером из числа допустимых в системе. Информация о произошедших в системе событиях распространяется при помощи механизма сообщений, встроенного в QNX. Генерировавший событие ПЗ по специально созданному для этого каналу посылает ПТ сообщение, содержащее в прикрепленной структуре данных список номеров генерируемых событий. Реакция остальных ПЗ на каждое конкретное событие определяется самими ПЗ. Для того чтобы определить ее, ПЗ должны заблаговременно послать ПТ специальное сообщение. Оно содержит в прикрепленной структуре список номеров событий, на которые данный ПЗ будет реагировать, и указание на один из двух возможных видов реакции. В первом случае ПЗ будет заблокирован до наступления одного из перечисленных событий; во втором – обратно немедленно вернется ответ, содержащий номера событий (из числа ранее запрошенных) которые произошли в системе с момента последнего запроса. Время, необходимое для генерации событий и организации реакции на них, измеряется микросекундами, поэтому механизм событий, реализованный в данном ПОСУ, является эффективным средством организации совместной работы относительно независимых ПЗ в жестком реальном времени.

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

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

Инициализация системы производится по информации, размещенной в двух файлах. В первом файле (PINDEX_TAB.dat) номерам событий и номерам ПЗ (называемых в данной системе pindex) ставятся в соответствие мнемонические обозначения и назначаются приоритеты всех ПЗ.

Во втором файле (TAB_NET_&_PROC.dat) находится информация о распределении ПЗ по компьютерам сети и о размещении общедоступных данных ПЗ в общей памяти ПТ. Записи оформлены в виде сетевой таблицы и таблицы процессов ПОСУ, доступных каждому ПТ.


Структура программного обеспечения системы управления двуногим шагающим манипуляционным роботом:
ИМ – исполнительный механизм

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

На рисунке представлена структура ПОСУ для проектируемого двуногого шагающего манипуляционного робота. В системе управления используются два компьютера. Первый бортовой компьютер (ПК1) имеет развитое периферийное оборудование. На нем выполняется ПЗ-таймер (timer), ПЗ, опрашивающий датчики, производящий цифровую фильтрацию и управляющий системой приводов робота (controller), а также ПЗ, обрабатывающие информацию системы ориентации (girow) и силомоментных датчиков (smd).

Второй стационарный компьютер (ПК2) связан с бортовым с помощью кабеля. Он предназначен для реализации верхнего уровня управления и взаимодействия с оператором. На нем выполняются ПЗ решения обратной задачи кинематики (back_task_1), формирования траектории движения стоп и центра масс робота (back_task_tfr), интерфейса оператора (back_task_tfr), регистрации экспериментальных данных (data_trans_tfr), записи экспериментальных данных на диск (data_to_file_tfr), чтения и записи на диск параметров системы приводов робота (drive_file). Данные об именах ПЗ, их назначении и используемых ими событиях представлены в таблице.

Данные об именах процессов-задач и используемых ими событиях

Имя и назначение ПЗ Генерируемые события и их назначение Ожидаемые события
back_task_1
решение обратной задачи кинематики
EVENT_NEED_NEXT_S_VECTOR
запрос следующего целевого вектора
EVENT_CONTROLLER_2
back_task_tfr
формирование траектории движения стоп и ЦМ
EVENT_NEW_S_VECTOR
целевой вектор готов
EVENT_NEED_NEXT_S_VECTOR
tfr_app
интерфейс оператора (работает в асинхронном режиме)
EVENT_TO_SAVE_DRIVE_PARAMETERS
требование сохранения параметров СУ
 
EVENT_DATA_TO_FILE_BEGIN
начать сохранение экспериментальных данных на диск
 
EVENT_DATA_TO_FILE_END
закончить сохранение экспериментальных данных на диск
 
EVENT_SOLVE_STATIC_BACK_TASK
начать решение обратной задачи (движение в режиме статической ходьбы)
 
EVENT_GO_TO_INITIAL
перевести робот в исходное положение
 
EVENT_SOLE_FORCES_TO_0
обнулить значения сигналов датчиков стоп
 
EVENT_STOP_MOVING
остановить движение
 
EVENT_CONTINUE_MOVING
продолжить движение
 
data_trans_tfr
регистрация экспериментальных данных
  EVENT_CONTROLLER_2
data_to_file_tfr
запись на диск экспериментальных данных
  EVENT_TIME_100
(генерируется каждые 0.1c)
EVENT_DATA_TO_FILE_BEGIN
EVENT_DATA_TO_FILE_END
drive_file
чтение и запись на диск параметров СУ
EVENT_END_DRIVE_FILE
запись закончена
EVENT_TO_SAVE_DRIVE_PARAMETERS
timer
генерация событий, предназначенных для привязки системы к реальному времени.
EVENT_TIME_1
выдается каждые 0.001с
 
EVENT_TIME_100
выдается каждые 0.1 с
 
EVENT_NET
инициация обмена событиями в сети каждые 0.01 с
 
EVENT_DATA_ASYNCHRO_CHANGE
инициация обмена данными в сети каждые 0.01 с
 
controller
опрос датчиков, цифровая фильтрация, управление системой приводов робота
EVENT_CONTROLLER_2
инициация синхронных с контроллером действий пониженной частоты (0.01с)
EVENT_TIME_1
(генерируется каждые 0.001c)
girow
обработка сигналов системы ориентации
  EVENT_CONTROLLER_2
(генерируется каждые 0.01c)
smd
обработка сигналов силомоментных датчиков стоп
  EVENT_CONTROLLER_2
(генерируется каждые 0.01c)

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

ЛИТЕРАТУРА

  1. Роб Кертен. Руководство по программированию приложений реального времени в QNX Realtime Platform. "Петрополис", сПб., 2001 г., 479 с.