Математика-3D позиционирование / мультилатерация
У меня есть проблема, связанная с 3d - позиционированием-что-то вроде GPS. Учитывая набор известных трехмерных координат (x, y, z) и их расстояния d от неизвестной точки, я хочу найти неизвестную точку. Опорных точек может быть сколько угодно, но их будет как минимум четыре.
Так, например, точки имеют формат (x, y, z, d). Я мог бы:
(1,0,0,1)
(0,2,0,2)
(0,0,3,3)
(0,3,4,5)
И здесь неизвестная точка была бы (0,0,0,0).
Как лучше всего это сделать? Есть ли существующее библиотека, поддерживающая мультилатерацию 3d? (Я не смог найти ни одного). Поскольку маловероятно, что мои данные будут иметь точное решение (все 4+ сферы, вероятно, не будут иметь ни одной идеальной точки пересечения), алгоритм должен быть способен аппроксимировать его.
До сих пор я думал о том, чтобы взять каждое подмножество из трех точек, триангулировать неизвестное на основе этих трех, а затем усреднить все результаты. Есть ли лучший способ сделать это?
4 ответов:
Вы можете использовать подход нелинейной оптимизации, определив функцию "затраты", которая включает в себя ошибку расстояния от каждой из ваших точек наблюдения.
Установив неизвестную точку в
(x,y,z), и рассматривая множествоNточек наблюдения(xi,yi,zi,di), можно использовать следующую функцию для характеристики полной ошибки расстояния:C(x,y,z) = sum( ((x-xi)^2 + (y-yi)^2 + (z-zi)^2 - di^2)^2 ) ^^^ ^^^ for all observation points i = 1 to NЭто сумма квадратов ошибок расстояния для всех точек в наборе. (На самом деле он основан на ошибке в квадрате расстояние, чтобы не было квадратных корней!)
Когда эта функция минимальна, целевая точка
Одним из способов минимизации этого типа уравнений был бы метод Ньютона . Вы должны были бы предоставить начальную начальную точку для итерации-возможно, среднее значение точек наблюдения (если они en-circle(x,y,z)будет находиться в оптимальном положении. Если решение даетC(x,y,z) = 0, то все наблюдения будут точно выполнены.(x,y,z)) или, возможно, начальное триангулированное значение из любых трех наблюдений. [16]}Edit: метод Ньютона-это итерационный алгоритм, который может быть использован для оптимизации. Простая версия будет работать в следующем направлении:H(X(k)) * dX = G(X(k)); // solve a system of linear equations for the // increment dX in the solution vector X X(k+1) = X(k) - dX; // update the solution vector by dX
G(X(k))обозначает вектор градиента, вычисленный вX(k), в этом случае:G(X(k)) = [dC/dx dC/dy dC/dz]
H(X(k))обозначает матрицу Гессена, вычисленную поX(k), в этом случае симметричная матрица 3x3:H(X(k)) = [d^2C/dx^2 d^2C/dxdy d^2C/dxdz d^2C/dydx d^2C/dy^2 d^2C/dydz d^2C/dzdx d^2C/dzdy d^2C/dz^2]Вы должны быть в состоянии дифференцировать функцию затрат аналитически, и поэтому в конечном итоге с аналитическими выражениями для
Другой подход-если вам не нравятся производные-заключается в том, чтобы аппроксимироватьG,H.G,Hчисленно, используя конечные разности.Надеюсь, это поможет.
Если вы можете потратить время, итерационное решение должно довольно быстро приблизиться к правильному решению. Поэтому выберите любую точку на правильном расстоянии от объекта а, затем обойдите набор, определяя расстояние до точки, а затем отрегулируйте точку таким образом, чтобы она находилась в том же направлении от объекта, но на правильном расстоянии. Продолжайте до тех пор, пока требуемая точность не будет достигнута (или пока точка больше не будет двигаться достаточно далеко в каждой итерации, чтобы она могла соответствовать вашей точности, согласно возможным эффектам приблизительных входных данных).
Для аналитического подхода я не могу придумать ничего лучшего, чем то, что вы уже предлагаете.
Нелинейные процедуры решения не требуются. Вы можете легко линеаризовать систему. Если взять парные различия
$(x-x_i)^2-(x-x_j)^2+(y-y_i)^2 - (y-y_j)^2+(z-z_i)^2-(z-z_j)^2=d_i^2-d_j^2$
Тогда бит алгебры даетлинейные уравнения
$(x_i-x_j) x +(y_i-y_j) y +(zi-zj) z=-1 / 2(d_i^2-d_j^2+ds_i^2-ds_j^2)$,
Где $ds_i$ - расстояние от датчика $i^{th}$ до начала координат. Это уравнения плоскостей, определяемые по формуле: пересечение сфер $i^{th}$ и $j^{th}$.
Для четырех датчиков вы получаете сверхдетерминированную линейную систему с уравнениями $4 = 2 = 6$. Если $A$ - результирующая матрица, а $b$ - соответствующий вектор RHS, то можно решить нормальные уравнения
$в^т р = ^т б$
Для вектора положения $r$. Это будет работать до тех пор, пока ваши датчики не будут копланарными.
Послушайте, Ньютон или Ньютон-Гаусс не будут работать. Это потому, что у вас есть разрыв в определенных точках пространства производных этой функции. Поверьте, я пробовал Ньютона-он почти никогда не сходится. Вместо этого я попробовал другой набор алгоритмов. Попробуйте использовать алгоритм прямого поиска google для оптимизации (поиска минимума или максимума функции). Это работает для меня - но иногда он сходится в неправильной точке-вероятно, из-за" плоскостности " 4 выбранных точек. В в этом случае-вы можете выбрать другую отправную точку-и вы, вероятно, получите правильный ответ. алгоритмический пример, который я нашел, был очень прост в реализации и работает удивительно хорошо. Надеюсь, что это помогло. Овации.
Comments