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

На рис. 3.1 приведены величины, которые определяются по разработанному алгоритму: g - угол, на который повернётся ось задних колёс автомобиля; dS - перемещение автомобиля на элементарном участке поворота; R1 и R2 - радиусы окружностей, описываемых задними колёсами; b - угол наклона вектора перемещения к предыдущей ориентации автомобиля (равен половине g). Радиус поворота центральной точки оси вращения задних колёс самосвала можно определить по формуле (3.2): R=R1+C/2 (3.2);
Определение текущего положения автомобиля (если известно предыдущее) выполняется следующим образом (определение ориентации см. рис 3.2):
Находим углы g (угол на который изменилась ориентация самосвала) и b (угол вектора элементарного перемещения в системе координат, связанной с самосвалом).
(3.3),
где r
- радиус
колеса, R
- радиус поворота
самосвала.
По
формуле
,
где b
- база самосвала,
a
- угол поворота
передних колёс,
мы можем вычислить
текущее значение
g
и сопоставить
с косвенно
измерянным.
b=g/2 (3.4);
Определяем dS (длина вектора элементарного перемещения):
dS=2*R*sin(b) (3.5);
Определяем коэффициент сцепления:
![]()
4) Уточняем dS с учётом коэффициента сцепления:
dS’=dS*к (3.7);
Определяем приращения координат в неподвижной системе:
dx=dS’*cos(q+b)
(3.8);
Определяем новую ориентацию автомобиля в неподвижной системе координат:
q‘=q+g (3.10).

В формулах 3.8 - 3.10: q‘- очередной угол ориентации самосвала в неподвижной системе координат; q - предыдущий угол ориентации самосвала в неподвижной системе координат.
Приведённые формулы достаточно сложны для реализации на языке программирования низкого уровня (даже с применением математического сопроцессора), поэтому желательно данный алгоритм реализовать на языках высокого уровня.
Нанесение на корту координат самосвала и привязка к направлению производится следующим образом:
на карте как занимаемый в данный момент времени указывается такой квадрат со стороной в одну дискрету, на котором находится центр задней оси самосвала
номер направления ориентации (с дискретностью 450) выбирается такой, к какому текущая ориентация самосвала ближе.
Данные вычисления можно выполнить лишь по готовности всех данных (К) от датчиков, поэтому есть смысл накапливать их в буферной схеме (специализированном контроллере, вставляемом в слоты бортовой ЭВМ), а по приходу данных от четырёх колёс послать сигнал прерывание в вычислительную систему. Драйверу, обрабатывающему запрос необходимо считать данные из этой буферной схемы и по ним произвести вычисления. Результаты определения положения по счислению следует оставлять в специально выделенной межпрограммной области связи, либо осуществлять выдачу последних готовых данных по запросу программ через вызовы программных прерываний.
1 n - дискретность выбора направлений