roboforum.ru

Технический форум по робототехнике.

 

Задачка навигации :)

Автомат, адаптивный автомат ... разум

Задачка навигации :)

Сообщение =DeaD= » 20 янв 2005, 20:28

Связана с навигацией по маякам :)

Имеем:
1. Данную "свыше" карту пространства с указанными на ней точными координатами маяков (координаты маяков заданы в 3х мерном пространстве, единица измерения - см. z=0 - плоскость пола, который считаем есть везде);

2. Информацию обо всех увиденных из данной точки маяках относительно нашего робота (т.е. центр робота - точка отсчета, а оси параллельны осям симметрии робота) - информация об направлении на маяк обладает высокой точностью (+/- 1 градус), а об расстоянии до маяка - низкой (+/- 15-20%);

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

Надо получить уточненные наши координаты с учетом всей имеющей-ся информации;

Ваши _конструктивные_ предложения?
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Сообщение =DeaD= » 20 янв 2005, 20:42

Мое решение:

Строим функцию 3 переменных - (x,y,a) - координаты робота и угол его направления, соответствующую величине непохожести положения указанного её переменными на наше текущее положение, а затем примитивно минимизируем ее по совокупности переменных.

Функция будет составной, т.е. состоять из суммы слагаемых, каждое из которых соответствует 1 известному факту, который будет проверяться, например, функция непохожести положения видимого маяка №4 из камеры на положение, которое будет видно из (x,y,a).

------------------------------------------------------------------------------
Построим компоненту функции соответствующая видимому маяку:

Пусть вектор B=(Bx,By,Bz) - абсолютные координаты маяка #i,
вектор V=(Vx,Vy,Vz) - в каких относительных координатах мы его видим;

Заранее расчитаем вектор nV=V/|V|=(nVx,nVy,nVz) - нормированный вектор V, а также длину вектора V, dV=|V|.

Теперь рассчитаем в каких локальных координатах W глядя из положения (x,y,a) будет находится маяк B:

W=(Wx,Wy,Wz)
Wx=cos(a)*(Bx-x)+sin(a)*(By-y)
Wy=cos(a)*(By-y)-sin(a)*(Bx-x)
Wz=Bz

Исходя из предыдущих рассуждений дальше нужно посчитать nW=W/|W|, и dW=|W|. После чего компонента функции выглядела бы так: F[V,B](x,y,a)=|nW-nV|*Pn+|dW-dV|*Pd, где Pn, Pd - параметр оценивающий "маловероятность" ошибки в оценке направления и расстояния соответственно. Однако учитывая что dW и nW будут иметь "страшный и запутанный" вид, можно попробовать как-то упростить подход к поиску наиболее вероятного расположения.

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

Тогда компонента функции будет выглядеть так: F[V,B](x,y,a)=|Wx-Vx|+|Wy-Vy|+|Wz-Vz|, или даже (чтобы не счиать производную модуля функции - F[V,B](x,y,a)=(Wx-Vx)^2+(Wy-Vy)^2+(Wz-Vz)^2. При этом последнее слагаемое всегда константа, поэтому можно  в нашей задаче минимизации ее отбросить.

Получаем компоненту функции соответствующую одному видимому маяку в виде:

F[V,B](x,y,a)  =  (cos(a)*(Bx-x)+sin(a)*(By-y)-Vx)^2  +  (cos(a)*(By-y)-sin(a)*(Bx-x)-Vy)^2.

Её производные:
F'x = -2*cos(a)*(cos(a)*(Bx-x)+sin(a)*(By-y)-Vx) + 2*sin(a)*(cos(a)*(By-y)-sin(a)*(Bx-x)-Vy)
F'y = -2*sin(a)*(cos(a)*(Bx-x)+sin(a)*(By-y)-Vx) - 2*cos(a)*(cos(a)*(By-y)-sin(a)*(Bx-x)-Vy)
F'a = 2*(cos(a)*(Bx-x)+sin(a)*(By-y)-Vx)*(cos(a)*(By-y)-sin(a)*(Bx-x)) - 2*(cos(a)*(By-y)-sin(a)*(Bx-x)-Vy)*(sin(a)*(By-y)+cos(a)*(Bx-x))

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

Поэтому мы сделаем совершенно хамский по отношению к точным наукам, но вполне позволительный на практике шаг - переберем все углы "a" внешним циклом, а во внутреннем будем искать наиболее реальные (x,y), а уже по найденным стоить штрафную функцию соответствующую тройке (x,y,a), и из всех таких полученных троек и значений функции выберем с наименьшим значением штрафной функции.

Тогда мы можем сказать следующее:

F[V,B,a](x,y)=(cos(a)*(Bx-x)+sin(a)*(By-y)-Vx)^2  +  (cos(a)*(By-y)-sin(a)*(Bx-x)-Vy)^2,

т.е. свернув коэффициенты:

F[V,B,a](x,y)=(A1*x+B1*y+C1)^2  +  (A2*x+B2*y+C2)^2,

где:
A1=-cos(a); B1=-sin(a); C1=cos(a)*Bx+sin(a)*By-Vx;
A2=-cos(a); B2=sin(a); C2=cos(a)*By-sin(a)*Bx-Vy;
или после перемножения и сложения:

F[V,B,a](x,y)=K20*x*x+K02*y*y+K11*x*y+K10*x+K01*y+K00,

где:
K20=A1*A1+A2*A2; K02=B1*B1+B2*B2; K00=C1*C1+C2*C2;
K11=A1*B1+A2*B2; K10=A1*C1+A2*C2; K01=B1*C1+B2*C2.

Далее считаем производные компоненты:

F'x=2*K20*x+K11*y+K10 и
F'y=2*K02*y+K11*x+K01

Вот мы и получили компоненты для построения системы из двух линейных уравнений с двумя неизвестными. А такие решать мы уже умеем.

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

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

Компонента соответствующая нашему знанию о предполагаемом месторасположении:

Пусть мы знаем что теоретически должны находится в координатах (tx,ty) и иметь угол направления ta, тогда

F[tx,ty,ta,a](x,y) = (ta-a)^2*Pa + (tx-x)^2*Pxy + (ty-y)^2*Pxy, где Pa - насколько точны наши данные об ожидаемом направлении, а Pxy - насколько точны наши данные об ожидаемых координатах.

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

F[tx,ty,ta,a](x,y) = K20*x*x+K02*y*y+K10*x+K01*y+K00,
где K20=Pxy, K02=Pxy, K10=-2*tx*Pxy, K01=-2*ty*Pxy,
K00=(ta-a)^2*Pa+(tx*tx+ty*ty)*Pxy

Производные компоненты:

F'x=2*K20*x+K10
F'y=2*K02*y+K01

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

Все, дальше для каждого случая перебираем переменную "а", составляем систему линейных уравнений F'x=0 & F'y=0, находим минимизирующие (x,y), рассчитываем штрафную функцию F(x,y,a), запоминаем ее с тройкой (x,y,a), далее из всех троек выбираем с минимальной штрафной функцией!

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

Если не убюсь с отладкой (хотя вроде и не такое отлаживал :) да еще в спешке), то за выходные сделаю программу эмулятор-полигон для отладки, демку и может библиотеку, не исключено, что даже часть выложить успею.
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Сообщение =DeaD= » 23 янв 2005, 19:27

Вот бывает напишешь глупость и никто не поправит :)

Короче все существенно проще, если кому будет надо - могу рассказать :)
Аватара пользователя
=DeaD=
 
Сообщения: 24053
Зарегистрирован: 06 окт 2004, 18:01
Откуда: Ебург
прог. языки: Pascal / C++ / PHP / 1C
ФИО: Антон Ботов

Сообщение Kanoka » 23 янв 2005, 20:01

DeaD это ж как минимум пол дня вникать нужно, кто тут попровит?
А как будет нужно мы спросим! Можешь не сомниватся :)
Kanoka
Модератор
 
Сообщения: 1271
Зарегистрирован: 11 ноя 2004, 03:18
Откуда: Москва


Вернуться в Алгоритмы

Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 1

Mail.ru counter