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

любая возможная ориентация самосвала.

Для ускорения планирования предполагается, что самосвал может

поворачивать на угол 450 . т. е. Мы получаем восемь возможных направлений

перемещения самосвала. Траектория строится по следующим критериям:

минимальная длина, минимальное количество повротов.

Алгоритм заключается в следующем:

1) составляется массив 8 х a x b, где a и b - стороны прямоугольника, в

который вписывается карта.

2) перед началом поиска этот массив заполняется нулями;

3) определяется точка конца траектории;

4) счётчик расстояния и искатель устанавливается на ноль;

5) из найденной точки делается шаг в любом возможном направлении;

6) если шаг параллельно осям координат - к счётчику расстояния прибавляется

10, иначе - 14;

7) значение счётчика записывается в ячейку N x X x Y, где N - направление;

X, Y - координаты текущей позиции;

8) искатель увеличивается на 1;

9) происходит поиск ячеек со значением равным искателю;

10) если такая ячейка найдена, то от неё делаются ходы во всех возможных

направлениях, при этом счётчики расстояний соответствующим образом

модифицируются и записываются в новые ячейки;

11) если искатель достиг входа увеличиваем счётчик достижений на 1, иначе

переход к пункту 8;

12) если счётчик достижений равен менее двух переходим к пункту 8;

13) обратный поиск маршрута: в точке входа находим направление, оценка

расстояния которого минимальна;

14) делаем ход навстречу этому направлению;

15) если достигли входа - конец;

16) отыскиваем направление, оценка расстояния в котором минимальна;

17) переход к пункту 14.

Реальный поворот самосвала на карте и в виде траектории моделируется

участком ломанной (приложение 2), содержащим излом на (450 (в зависимости

от направления) предполагается, что поворот начинается (заканчивается)

раньше (позже) точки излома для ортогонального перемещения на одну

дискрету, а для наклонного к осям координат на три. Вид поворота на карте

приведён на рис. 2.1. В связи с этим длина прямого горизонтального участка

допускается не менее трёх шагов, а наклонного - не менее семи шагов.

Приблизительный вид массива в конце траектории по окончании работы

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

через закрашенные клетки закрашенные клетки). В результате работы алгоритма

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

соседние звенья которой наклонены друг относительно друга на угол (450 .

Время работы этого алгоритма на процессоре PENTIUM S - 75 приблизительно

0,2 с., если учесть, что для проезда от начала до конца требуется запускать

алгоритм лишь однажды, то быстродействие его можно считать достаточным.

[pic]

Описанный алгоритм применим не только к самосвалу, но и после

геометрической адаптации к любому транспортному средству, в частности он

позволит двигаться транспортному роботу в недетерминированной (не разбитой

на кварталы) среде.

В связи с тем, что поиск конкретного числа в трёхмерном массиве,

содержащем десятки тысяч чисел слишком долог, был применён стековый метод

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

данных программы выделено шестнадцать одинаковых областей для хранения

данных. Эти области поочерёдно заполняются данными о координатах,

направлении и длине последнего ровного участка. При обработке одна из

шестнадцати областей служит источником данных, а остальные накапливают

информацию о новых достигнутых клетках. По окончании обработки одной

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

обработанная только что используется для накопления следующей порции

данных. Для упрощения обслуживания областей используется массив

дескрипторов, в котором хранится информация об адресе области и о глубине

её заполнения, а также введён специальный флаг, который устанавливается при

исчерпании текущей области стека.

| | | | | | | | | | | | | | | | | | | | |61| | |7| | |8|

| | | | | | | | | | | | | | | | | | | | | | | |1| | |1|

|1|19| | |1A/| | |1B/|1| |1C/|2| |1D/|3| |1E/|4| |1F/|51| |20|6| |21|7|

|1|/1| | |1 | | |1 |1| |1 |1| |1 |1| |1 |1| |1 | | |/1|1| |/1|1|

| | | | | | | | | | | | | | | | | | | | | | | | | | | |

| | | | | | | | | | | | | | | | | | | | | | | | | | | |

| |19| | |1A/| | |1B/| | |1C/|2| |1D/|3| |1E/|4| |1F/|56| |20|6| |21|7|

| |/2| | |2 | | |2 | | |2 |6| |2 |6| |2 |6| |2 | | |/2|6| |/2|6|

|1| | | |11 | | | |1| | |2| | |3| | |4| | |56| | |6| | |7|

|5| | | | | | | |5| | |6| | |6| | |6| | | | | |6| | |6|

| | | | | | | | | | | | | | | | | | | | | | | | | | | |

| | | | | | | | | | | | | | | | | | | |1F/| | |20| | |21| |

| | | | | | | | | | | | | | | | | | | |3 | | |/3| | |/3| |

| | | | | | | | | | | | | | | | | | | | |61| | |7| | |8|

| | | | | | | | | | | | | | | | | | | | | | | |1| | |1|

| | | | | | | | | | | | | | | | | | | | | | | | | | | |

| | | | | | | | | | | | | | | | | | | | | | |20| | |21| |

| | | | | | | | | | | | | | | | | | | | | | |/4| | |/4| |

| | | | | | | | | | | | | | | | | | | | | | | |7| | |8|

| | | | | | | | | | | | | | | | | | | | | | | |5| | |5|

| | | | | | | | | | | | | | | | | | | | | | | | | | | |

| | | | | | | | | | | | | | | | | | | | | | | | | |21| |

| | | | | | | | | | | | | | | | | | | | | | | | | |/5| |

| | | | | | | | | | | | | | | | | | | | | | | | | | |8|

| | | | | | | | | | | | | | | | | | | | | | | | | | |9|

Рис 2.2. Приблизительный вид массива в конце траектории по окончании работы

алгоритма.

Программная реализация этого алгоритма выполнена на Ассемблере. Этот

язык программирования выбран по тому, что программы разработанные на нём

обладают наибольшим быстродействием и занимают минимальный объем памяти.

Языки высокого уровня не смогут обеспечить требуемое быстродействие (размер

программы не критичен) и содержат внутренние ошибки не поддающиеся

устранению.

2.2 Сглаживание поворотов.

По окончании первичного планирования траектории (п 2.1) мы будем

иметь предварительную траекторию, представляющую собой ломанную, каждое из

звеньев которой повёрнуто на угол (450 относительно предыдущего звена.

Данная траектория имеет низкое качество по сравнению с достижимой, поэтому

требуется произвести сглаживание («срезание углов»). Именно это и будет

выполнятся при вторичном планировании.

В целях экономии времени, алгоритм сглаживания будет запускаться для

ближайшего поворота (как исключение и для следующего). При сглаживании

программа будет пытаться провести траекторию между любыми двумя точками до

начала ближайшего поворота и после его окончания. Для повышения

быстродействия программа начнёт с минимального сглаживания (малое удачное

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

сглаживания. Попытки «срезать угол» будут прекращены как только исчезнет

возможность осуществить сглаживание. Отрезок сглаживания будет проводится

через клетки на которых возможно нахождение центра самосвала и ориентация

самосвала в любом направлении.

В результате удачного сглаживания (рис.2.3 - 2.5), вместо поворотов на

(450 возникнут два поворота на угол менее 450 и отрезок сглаживания, длина

которого будет меньше суммарной длины всех частей ломанной, расположенных

между концами отрезка сглаживания. В связи с тем, что в программу,

выполняющую первую стадию планирования не заложены повороты на эти углы ((1

и (2), алгоритм сглаживания по формулам должен будет определить точку

начала поворота (r1 и r2). Указанные на рис. 3.3 величины найдём по

формулам 2.1 - 2.5.

(2.1);

[pic]

(1=arccos[pic] (2.2);

(2=arccos[pic] (2.3);

r1=R*tg((1/2) (2.4);

r2=R*tg((2/2) (2.5).

В случае поворота, изображенного на рисунке 2.4, величины, его

определяющие, рассчитаем по формуле 2.6 и 2.7:

(=arctg[pic] (2.6)

[pic] (2.7).

Как в первом, так и во втором случаях линия l проводится таким образом,

чтобы она не проходила через клетки, на которые запрещён въезд самосвала. В

случае для рисунка 2.6 по формулам 2.8 и 2.9: [pic] (2.8);

(=arctg[pic] (2.9).

Возможна ситуация, когда конец сглаживания поворота совпадает с

началом сглаживания следующего поворота (рис 2.6). В этом случае угол ((5)

между отрезками сглаживания l1 и l2 вычисляется по формуле 2.7, а

упреждение поворота по формуле 2.4 (2.5), но с (5 вместо (2 ,

[pic]

В случае, если сглаживание данного поворота произвести не удалось, то

начинать поворот требуется на одну дискрету раньше точки излома на карте

(при до поворота движении параллельно осям

[pic]

координат), и на три дискреты раньше при движении до поворота под углом к

осям. Таким образом rпарр =1,875м (округлим до 2 м), а rподуглом =4,375 м

(округлим до 4,5 м). В связи с тем, что реальная ориентация самосвала перед

сглаживанием будет отличаться от фиксированной, требуется корректировать

углы на которые должен повернуть самосвал и расстояния которые он должен

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

приведённых ниже рисунках (2.7 а, б, в, г) и формулах введены следующие

обозначения: (’ - угол между направлениями планируемом до поворота

траектории и реальной ориентацией самосвала; ( - угол между направлением

планируемой траектории и отрезком сглаживания; ( - угол между реальным

направлением самосвала и отрезком сглаживания; х - реальное расстояние до

поворота; k - количество дискрет до излома; d -длина одной дискреты.

Методика выбора формулы для расчёта данных коррекции приведена в таблице

2.1.

В отличие от алгоритма первичного планирования траектории (п. 2.1),

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

уровня, располагающими библиотеками математических функций и позволяющих

выполнять действия над данными разных типов.

[pic]

Таблица 2.1. Выбор расчётных формул для получения данных коррекции

|Номер |Формула для вычисления ( |Формула для вычисления х |

|рисунка | | |

|2.8 а |(=(-(’ |х=kd*sin(()/sin(() |

|2.8 б |(=(+(’ |х=kd*sin(()/sin(() |

|2.8 в |(=(-(’ |х=1.41*kd*sin((+45)/sin(() |

|2.8 г |(=(-(’ |х=1.41*kd*sin((-45)/sin(() |

[pic]

2.3 Планирование траектории в зоне разворота

и около точки разгрузки.

К месту разгрузки самосвал должен подъезжать задним ходом, однако

максимум расстояния в местах разгрузки он проезжает передним ходом

следовательно, системе автовождения самосвала требуется определить

координаты точки смены направления движения и точек сопряжения разных

участков траектории, кроме того система автовождения должна заранее

располагать части траектории на местности, чтобы предусмотреть и избежать

наезды на препятствия и въезды в зоны, не определяемые как препятствия.

Планирование смены направления в местах разгрузки самосвала имеет

следующие трудности:

n вероятностный характер координат и направления последнего

прямого участка движения передним ходом;

n высокие требования (в связи с необходимостью повысить безопасность) к

точности траектории движения задним ходом и высокие требования к точности

определения координат конца участка движения задним ходом

n необходимость минимизации суммарной длины отрезка до смены переднего хода

на задний и длины отрезка движения задним ходом;

n ограничения на предельную длину отрезка движения задним ходом;

В связи с условиями, приведёнными выше точка смены направления должна

выбираться по следующим критериям:

1) Участок движения задним ходом должен быть кратчайшим.

Страницы: 1, 2, 3