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

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

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

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

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

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

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

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

В результате удачного сглаживания (рис.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) Участок движения задним ходом должен быть кратчайшим.

2) Движению до и после реверса не должны мешать препятствия.

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

4) Автомобиль не должен покидать пределы рабочей площадки.

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

местом точек (центров самосвала) смены направления движения при подъезде к

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

наклона в точке максимально удалённой от точки разгрузки равный 0, а в

точке, ближайшей к точке разгрузки - 900 . Такими свойствами обладает

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

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

на расстоянии большем или равным половине ширины самосвала от

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

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

для исправления неточности ориентации самосвала (ограничивает малую

полуось), была перпендикулярна границе рабочей площадки в точке разгрузки.

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

этому эллипсу как по заранее определённому участку траектории, используя

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

Планирование траектории начинается с построения эллипса (уравнение

имеет вид (2.10)).

[pic] (2.10).

Большая его полуось (a) выбирается исходя из ширины площадки для манёвра,

а малая (b) таким образом, чтобы наименьший радиус кривизны эллипса был не

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

для определения точки разворота. Сопрягающая дуга является частью

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

строится так, чтобы она касалась эллипса и прямого отрезка, от которого и

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

Ширину малой полуоси можно определить следующим образом. Уравнение

эллипса может быть приведено к виду: [pic]

Уравнение первой производной по х будет иметь вид: [pic].

Уравнение второй производной по х будет иметь вид: [pic]

Текущий радиус эллипса

[pic] .

Подставив в последнее выражение х=0, получим [pic]

С учетом ограничения [pic] получим:

[pic].

Опорными точками этого участка траектории являются: точка А с

координатами x1, y1; точка О с координатами x2, y2 и точка В с координатами

x3, y3.

Геометрическим местом центров окружностей частью которых является дуга

сопряжения является кривая близкая к эллипсу, имеющему уравнение [pic] что

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

перехода прямой отрезок - дуга сопряжения и дуга сопряжения - эллипс.

Радиус R дуги сопряжения должен быть незначительно больше технологического

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

самосвала.

При развороте самосвала система ориентации должна работать как

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

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

постоянная.

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

должен двигаться по прямой. Программа должна заранее вычислить точки

перехода прямая - сопрягающая окружность (А) и сопрягающая окружность -

эллипс (В, точка смены направления), и по достижении указанных точек (с

учётом погрешности) соответствующим образом менять задающие воздействия.

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

найдены из условий:

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

окружности в этой точке должны быть равны;

n окружность и эллипс должны иметь общую точку;

n производные окружности и эллипса в этой точке должны быть равны.

Радиус (R) дуги сопряжения должен быть незначительно больше

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

радиуса поворота самосвала.

Геометрические особенности планирования разворота и подъезда самосвала

к месту разгрузки изображены на рисунке 2.8.

Приведённые на рисунке 2.8 величины можно определить по формулам 2.11

- 2.20:

A1=[pic] (2.11);

[pic]

A2=[pic] (2.12);

A=(a+R)2+k*(b+R)2 (2.13);

B=2*A1*(a+R)2-2*k*A2*(b+R)2 (2.14);

C=(a+R)2*( A12-(b+R)2 )+ A2*(b+R)2 (2.15);

X1=[pic] (2.16);

X2=X1-A1 (2.17);

Y2=k*X1-A2 (2.18);

Y3=[pic] (2.19);

(=arccos(1-[pic]) (2.20);

В формулах 2.13 - 2.22 введены промежуточные величины (А, А1, А2, В, С),

облегчающие программную реализацию вычислений.

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

самосвалом точки с абсциссой X=b+2*R. В ходе вычислений, которые должны

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

(X1, Y1) и (X3, Y3). Алгоритм позволяет реализовать автоматическое

планирование участка смены направления движения на бортовой ЭВМ системы

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

осуществлять вычисления за минимальное время.

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

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

2.4 Планирование выезда самосвала

из зоны разгрузки.

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

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

направления движения. Планирование траектории также должно состоять из

первичного и вторичного.

Следует отметить, что траектория движения должна быть прижата к

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

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

разгрузиться. Для алгоритма первичного планирования (п 2.1) это выливается

лишь во введении дополнительных критериев оценки. Сглаживание поворотов

(вторичное планирование) должно выполнятся по аналогичным (п. 2.2)

формулам.

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

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

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

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

скорости и радиуса поворота от скорости. Информация о выезде из зоны

манёвров может быть передана от ДЭВМ или заранее заложена в память как и

карта местности.

3 Навигация большегрузного

карьерного автосамосвала по счислению.

В связи с невозможностью точно измерять мгновенное положение

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

радионавигации карьерного самосвала, необходимо обеспечить программное

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

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

площадки). Указанные параметры особенно важны во время планирования

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

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

координатами при планировании подъезда к точке разгрузки.

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

мгновенное значение реального радиуса поворота. Реальный радиус поворота

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

на каждом колесе и работающие независимо друг от друга. Если между

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

угол, накапливать импульсы высокой частоты, то по простым формулам можно

определить радиус поворота (формула (3.1)):

[pic] (3.1).

В формуле: К1 - количество импульсов, пришедших между сигналами от датчика

поворота от заднего колеса, ближнего к центру поворота, С - ширина колеи,

К2 - количество импульсов, пришедших между сигналами датчика поворота от

заднего колеса, дальнего от центра поворота.

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

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

самосвалом одной дискреты. Длина одной дискреты 1,25 м, наибольшая

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

положении самосвала должны быть готовы:

t=1.25(м)/(15000(км/ч)/3600(с/ч))=0.3 с. Учитывая специфику групповой

работы датчиков уменьшим время ещё в 3 раза (запас времени на расчёты) и

получим t=0.1 с. Определим, на какой угол (() повернётся за это время

колесо самосвала (и шаг угловой сетки на датчике):

(=0,1(с)*4,167(м/с)/3(м)=0,1389 рад=7,950 , округлим полученное значение до

ближайшего числа на которое 360 делится без остатка и получим 80 (полос на

датчике должно быть 45). Если применить мультипликатор или повышающую

цепную передачу, то габариты датчика и количество дискрет на нём уменьшится

в i (передаточное отношение передачи) раз. Частоту импульсов, накапливаемых

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

сигналов от датчиков равна 10Гц., для обеспечения соответствия разрядности

сигнала разрядности (8 разрядов) препроцессора обрабатывающего сигналы от

датчиков выберем частоту 2560 Гц. Данная частота может быть получена не

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

количества тактов в циклах процессора.

[pic]

На рис. 3.1 приведены величины, которые определяются по разработанному

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

- перемещение автомобиля на элементарном участке поворота; R1 и R2 -

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

перемещения к предыдущей ориентации автомобиля (равен половине (). Радиус

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

определить по формуле (3.2): R=R1+C/2 (3.2);

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

выполняется следующим образом (определение ориентации см. рис 3.2):

1) Находим углы ( (угол на который изменилась ориентация самосвала) и (

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

самосвалом).

[pic] (3.3),

где r - радиус колеса, R - радиус поворота самосвала. По формуле [pic], где

b - база самосвала, ( - угол поворота передних колёс, мы можем вычислить

текущее значение ( и сопоставить с косвенно измерянным.

(=(/2 (3.4);

2) Определяем dS (длина вектора элементарного перемещения):

dS=2*R*sin(() (3.5);

3) Определяем коэффициент сцепления:

4) Уточняем dS с учётом коэффициента сцепления:

dS’=dS*к (3.7);

5) Определяем приращения координат в неподвижной системе:

dx=dS’*cos((+() (3.8);

6) Определяем новую ориентацию автомобиля в неподвижной системе координат:

(‘=(+( (3.10).

[pic]

В формулах 3.8 - 3.10: (‘- очередной угол ориентации самосвала в

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

неподвижной системе координат.

Приведённые формулы достаточно сложны для реализации на языке

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

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

высокого уровня.

Нанесение на корту координат самосвала и привязка к направлению

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

n на карте как занимаемый в данный момент времени указывается такой квадрат

со стороной в одну дискрету, на котором находится центр задней оси

самосвала

n номер направления ориентации (с дискретностью 450) выбирается такой, к

какому текущая ориентация самосвала ближе.

Данные вычисления можно выполнить лишь по готовности всех данных (К)

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

(специализированном контроллере, вставляемом в слоты бортовой ЭВМ), а по

приходу данных от четырёх колёс послать сигнал прерывание в вычислительную

систему. Драйверу, обрабатывающему запрос необходимо считать данные из этой

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

положения по счислению следует оставлять в специально выделенной

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

данных по запросу программ через вызовы программных прерываний.

-----------------------

[1] n - дискретность выбора направлений

-----------------------

[pic]

к= (пи1 *R1/( R12+b2 *(зи1) (3.6);

dy=dS’*sin((+()= (dS’)2 -(dx)2 (3.9);

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