3.12.Следование по маршруту
3.12.1.Вычисление направления на точку маршрута
Впредыдущих разделах описан процесс вычисления текущей ориентации роботизированного устройства в пространстве. Результаты данных вычислений используются в процессе передвижения роботизированного устройства.
Так как максимальные расстояния, преодолеваемые роботизированным устройством в процессе передвижения по маршруту небольшие и составляют порядка единиц километров, то задача вычисления направления на точку маршрута значительно упрощается, так как карту можно условно представить плоской, пренебрегая преобразованиями, связанными с шарообразной формой Земли.
Вданном случае направление на точку маршрута определяется как обычное вычисление угла между двумерными векторами. Формула для вычислений имеет следующий вид
|
|
|
|
= |
2 |
1 2 2 |
12 |
2 |
2 |
, |
|
|
|
(3.44) |
1 |
|
1 |
1 |
+ 1 |
2 |
+ 2 |
2 |
|
2 |
|
||||
|
где |
|
азимут движения от текущего местонахождения к точке маршру- |
|||||||||||
та; |
и |
|
––широта и долгота текущего местонахождения; |
|
и |
|
– широта и |
|||||||
долгота целевого местонахождения.
Таким образом, решена задача вычисление азимута на целевую точку маршрута, приведены формулы для его вычисления. Произведено упрощение вычислений за счет учета максимальной дистанции перемещения.
3.12.2. Удержание направления на точку маршрута
После того, как направление на целевую точку маршрута вычислено, роботизированное устройство производит поворот корпуса на полученный угол и начинает движение вперед. Во время движения траектория роботизированного устройства может измениться под действием внешних факторов. Например, если роботизированное устройство движется по наклонной либо скользкой поверхности. Таким образом, необходимо средство оперативной коррекции курса роботизированного устройства в реальном времени. Для этих целей используется автоматизированная система управления на основе ПИД-регулятора.
Пропорционально-интегрально-дифференцирующий (ПИД) регулятор — устройство в управляющем контуре с обратной связью. Используется в системах автоматического управления для формирования управляющего сигнала с целью получения необходимых точности и качества переходного процесса. ПИД-регулятор формирует управляющий сигнал, являющийся суммой трёх
84
слагаемых, первое из которых пропорционально разности входного сигнала и сигнала обратной связи (сигнал рассогласования), второе — интеграл сигнала рассогласования, третье — производная сигнала рассогласования.
На рис. 3.19 представлены основные узлы системы автоматизированного управления роботизированным устройством.
X Y Z |
Блок 3Д- |
X Y Z |
|
|
|
«Сырые» данные |
коррекции |
Полезные данные |
|
||
магнитометра |
|
магнитометра |
X Y Z |
Блок 3Д- |
X Y Z |
|
|
|
«Сырые» данные |
коррекции |
Полезные данные |
|
||
гироскопа |
|
гироскопа |
X Y Z |
Блок 3Д- |
X Y Z |
|
|
|
«Сырые» данные |
коррекции |
Полезные данные |
|
||
акселерометра |
|
акселерометра |
ДШ
Данные текущего |
|
|
местоположения |
Блок |
Н Д |
|
||
|
обработки |
|
|
координат |
Информация |
Д Ш |
|
о маршруте |
|
|
|
Данные целевого |
|
|
местоположения |
|
|
Фильтр
Маджвика
Регулятор
направления
КТ Р
Данные ориентации (крен, тангаж, рысканье)
Блок смешивания 
и ограничения
Регулятор
положения
ЛП
ШИМ-сигнал для электродвигателей
Рис. 3.19. Система автоматизированного управления роботизированным устройством
Частично функционирование данной системы уже описано в предыдущих разделах. А именно, «сырые» данные инерциального датчика после прохождения блока трехмерной коррекции (содержит данные калибровки датчика) попадают в фильтр Маджвика. После вычислений на выходе фильтра имеем кватернион, который преобразуется в углы Эйлера (крен, тангаж и рысканье). Кроме того, роботизированное устройство также производит измерение текущего глобального местоположения устройства. В блоке обработки координат происходит вычисление азимута на точку назначения. Вычисленный азимут и текущая ориентация роботизированного устройства подаются в регулятор направления (ПИД), который формирует сигналы управления для электроприводов.
Таким образом, рассмотрен процесс коррекции курса роботизированного устройства в реальном времени. Описаны применяемые технологии, а также общая структура автоматизированной системы управления.
85