Простой алгоритм трилатерации в моделируемом трехмерном пространстве

Question

Контекст: я работаю над внедрением навигационной системы для мобильных компьютеров, добавленной OpenComputers, модом Minecraft. Для тех, кто не знаком с модом, он в основном добавляет множество программируемых на Lua модернизируемых компьютеров, в том числе мобильных, а именно роботов, дронов и планшетов. Одна из многих проблем, часто возникающих при попытке запрограммировать роботов и дронов на выполнение автономной задачи, заключается в том, чтобы они всегда знали свои координаты.

Простейшим решением было бы использовать обновление навигации, которое делает именно это - предоставляет компьютеру точные координаты относительно центра карты, с которой он был создан. Однако у него есть два основных недостатка - он занимает слот обновления Tier II, что немаловажно и ограничено областью карты. Последнее более или менее приемлемо, но все же делает этот метод навигации недоступным для некоторых случаев использования.

Другим решением было бы заставить компьютеры запоминать свои координаты один раз, а затем отслеживать их перемещения, но это также имеет ряд потенциальных предостережений - вы должны контролировать все перемещения с помощью пользовательских подпрограмм или использовать хаки для перехвата вызовов компонентов, вы можете ' Если компьютер перемещается без необходимости каждый раз вводить координаты вручную, существуют некоторые ошибки точности для дронов, и это не будет работать вообще для планшетов.

Третий метод - тот, над которым я работаю, - похож на реальный GPS. Это основано на том факте, что компьютеры можно модернизировать с помощью беспроводных сетевых карт, чтобы они могли отправлять сообщения друг другу на довольно большое расстояние в 400 блоков, и вместе с самим сообщением они получают точное расстояние (число с плавающей запятой, в блоках).) между отправителем и получателем. Если мы обозначим некоторые стационарные компьютеры как "спутники", которые постоянно транслируют свое местоположение, мы можем сделать мобильный компьютер способным трилировать свое точное местоположение, используя информацию со спутников 4+.

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

Проблема: мне нужно найти алгоритм трилатерации (в идеале идет с примером кода), который позволил бы любому мобильному компьютеру вычислить свою позицию (с погрешностью ~0,25 блоков), зная координаты назначенных "спутников" и расстояния им. Мы предполагаем, что все компьютеры и спутники оснащены беспроводными картами Tier II (то есть они могут отправлять сообщения друг другу в общем диапазоне 400 блоков и знать расстояние между отправителем и самим собой с точностью, допускаемой числами float32). Решение будет написано на чистом Lua без доступа к сторонним сервисам, поэтому такие пакеты, как Mathematica, не нужны. В настоящее время я держу пари на каком-то подходящем методе, хотя я не знаю, как его реализовать и насколько хорошо он может быть адаптирован к возможности того, что некоторые спутники в диапазоне передают неправильную позицию.

На самом базовом уровне мы можем предположить, что есть 4 спутника, которые постоянно и правильно передают свое положение, расположены на небольшом расстоянии друг от друга и не лежат в одной плоскости 2D. Есть некоторые дополнительные условия, к которым в идеале должен быть адаптирован алгоритм - см. Раздел ниже.

Бонусные баллы за:

• Делая алгоритм достаточно маленьким, чтобы поместиться в 2 КБ памяти беспилотника (при условии кодировки UTF8). Тем не менее, он должен занимать гораздо меньше места, чтобы и основная программа тоже подходила. Чем меньше, тем лучше.
• Создание алгоритма, который позволяет спутникам находиться очень близко друг к другу и иметь нецелые координаты (чтобы можно было заменить несколько фиксированных спутников одним постоянно движущимся роботом или дроном, или чтобы сам мобильный компьютер двигался, когда он выполняет измерения от один спутник).
• Создание алгоритма, который позволяет присутствовать менее чем 4 спутникам, предполагая, что положение уже может быть определено - например, если рассматриваемый мобильный компьютер является роботом, и все возможные позиции, кроме одного, находятся ниже или выше допустимого диапазона высоты для блоков (у<0 или у>255). Такая настройка возможна, если на высоте, скажем, y=255 расположены три спутника.
• Создание алгоритма, который устойчив к некоторым спутникам, передающим немного неправильное положение (небольшая ошибка в настройке). При наличии достаточного количества правильных измерений алгоритм должен определить правильное положение или вывести ошибку. В идеале он также может регистрировать местоположение "выключенного" спутника.
• Создание алгоритма, который устойчив к одновременному присутствию двух или более групп спутников, корректно транслирующих свои позиции в разных системах координат (большая ошибка в настройке). Каждая сеть имеет (предположительно уникальный) идентификатор, который позволяет различать разные сети, независимо настроенные разными игроками (или, ну, просто одной). Однако, если они не удосужились правильно установить идентификаторы, разные сигналы могут смешиваться, что приводит в замешательство мобильный компьютер. Следовательно, устойчивый алгоритм должен уметь обнаруживать эту ситуацию и либо просто выдавать ошибку, либо различать разные сети (тогда он может быть настроен в соответствии с целями конкретного приложения - то есть отказаться от загрузки, выбрать ближайшую сеть, выбрать самую большую сеть, запросить пользователя или управляющий сервер и т. д.).

Что я пробовал: Помимо попыток решить проблему самостоятельно, я также попытался найти подходящее решение в Интернете. Однако ни одно из решений, которые я смог найти, не подходило для этой задачи.

• Большинство вещей, которые я нашел, прибегая к помощи "алгоритмов трилатерации", имели дело с реальными системами GPS - то есть, используя только 2 координаты, строго учитывая ошибки и не давая достаточной точности в целом.
• Некоторые, напротив, были чисто математическими, предлагая построить ряд уравнений, чтобы найти точки пересечения сфер. К сожалению, насколько мой слабый математический фон позволяет мне понять, этот подход не учитывает погрешности точности плавающих чисел - окружности не совсем пересекаются, точки не совсем в одинаковых местах, и поэтому уравнения не имеют решений.
• Некоторые, казалось, объясняли решение, но включали много сложной математики, которую я не мог понять, и не включали точный алгоритм или, по крайней мере, пример кода.
• По крайней мере, один использовал внешние пакеты, такие как Mathematica, которые, опять же, недоступны в этом случае.

Если я оставил некоторые важные моменты неясными, пожалуйста, оставьте комментарий, чтобы я мог улучшить вопрос. Заранее спасибо!

Answer 1

Функция trilateration ожидает список спутников (их координаты и расстояния от мобильного компьютера) и предыдущие координаты мобильного компьютера.
Соберите только спутники из вашей собственной группы, исключите спутники из всех других групп.
Некоторые из ваших спутников могут отправлять неверные данные, это нормально.

Если доступных спутников недостаточно, функция возвращает nil так как он не может определить текущую позицию.
В противном случае функция возвращает текущие координаты мобильного компьютера, а список индексов спутников был признан неверным.
В случае неоднозначности новая позиция выбирается как ближайшая к предыдущей позиции мобильного компьютера.
Выходные координаты целые, координата Y ограничена диапазоном 0..255

Для правильного трилатерации должны быть выполнены следующие условия:

• (number_of_correct_satellites) должно быть> = 3
• (number_of_correct_satellites) должно быть>= 4, если существует хотя бы один неверный спутник
• (number_of_correct_satellites) должно быть> (number_of_incorrect_satellites)

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

do
   local floor, exp, max, min, abs, table_insert = math.floor, math.exp, math.max, math.min, math.abs, table.insert

   local function try_this_subset_of_sat(satellites, is_sat_incorrect, X, Y, Z)
      local last_max_err, max_err = math.huge
      for k = 1, math.huge do
         local oldX, oldY, oldZ = X, Y, Z
         local DX, DY, DZ = 0, 0, 0
         max_err = 0
         for j = 1, #satellites do
            if not is_sat_incorrect[j] then
               local sat = satellites[j]
               local dx, dy, dz = X - sat.x, Y - sat.y, Z - sat.z
               local d = (dx*dx + dy*dy + dz*dz)^0.5
               local err = sat.distance - d
               local e = exp(err+err)
               e = (e-1)/(e+1)/(d+1)
               DX = DX + dx*e
               DY = DY + dy*e
               DZ = DZ + dz*e
               max_err = max(max_err, abs(err))
            end
         end
         if k % 16 == 0 then
            if max_err >= last_max_err then
               break
            end
            last_max_err = max_err
         end
         local e = 1/(1+(DX*DX+DY*DY+DZ*DZ)^0.5/max_err)
         X = X + DX*e
         Y = max(0, min(255, Y + DY*e))
         Z = Z + DZ*e
         if abs(oldX - X) + abs(oldY - Y) + abs(oldZ - Z) <= 1e-4 then
            break
         end
      end
      return max_err, floor(X + 0.5), floor(Y + 0.5), floor(Z + 0.5)
   end

   local function init_set(is_sat_incorrect, len, ctr)
      for j = 1, len do
         is_sat_incorrect[j] = (j <= ctr)
      end
   end

   local function last_combination(is_sat_incorrect)
      local first = 1
      while not is_sat_incorrect[first] do
         first = first + 1
      end
      local last = first + 1
      while is_sat_incorrect[last] do
         last = last + 1
      end
      if is_sat_incorrect[last] == nil then
         return true
      end
      is_sat_incorrect[last] = true
      init_set(is_sat_incorrect, last - 1, last - first - 1)
   end

   function trilateration(list_of_satellites, previous_X, previous_Y, previous_Z)
      local N = #list_of_satellites
      if N >= 3 then
         local is_sat_incorrect = {}
         init_set(is_sat_incorrect, N, 0)
         local err, X, Y, Z = try_this_subset_of_sat(list_of_satellites, is_sat_incorrect, previous_X, previous_Y, previous_Z)
         local incorrect_sat_indices = {}
         if err < 0.1 then
            return X, Y, Z, incorrect_sat_indices
         end
         for incorrect_ctr = 1, min(floor((N - 1) / 2), N - 4) do
            init_set(is_sat_incorrect, N, incorrect_ctr)
            repeat
               err, X, Y, Z = try_this_subset_of_sat(list_of_satellites, is_sat_incorrect, previous_X, previous_Y, previous_Z)
               if err < 0.1 then
                  for j = 1, N do
                     if is_sat_incorrect[j] then
                        table_insert(incorrect_sat_indices, j)
                     end
                  end
                  return X, Y, Z, incorrect_sat_indices
               end
            until last_combination(is_sat_incorrect)
         end
      end
   end
end

Пример использования:

-- assuming your mobile computer previous coordinates were 99 120 100
local previous_X, previous_Y, previous_Z = 99, 120, 100
-- assuming your mobile computer current coordinates are 111 112 113
local list_of_satellites = {
   {x=22, y=55, z=77, distance=((111-22)^2+(112-55)^2+(113-77)^2)^0.5},  -- correct satellite
   {x=35, y=99, z=42, distance=((111-35)^2+(112-99)^2+(113-42)^2)^0.5},  -- correct satellite
   {x=44, y=44, z=44, distance=((111-94)^2+(112-94)^2+(113-94)^2)^0.5},  -- incorrect satellite
   {x=10, y=88, z=70, distance=((111-10)^2+(112-88)^2+(113-70)^2)^0.5},  -- correct satellite
   {x=54, y=54, z=54, distance=((111-64)^2+(112-64)^2+(113-64)^2)^0.5},  -- incorrect satellite
   {x=91, y=33, z=15, distance=((111-91)^2+(112-33)^2+(113-15)^2)^0.5},  -- correct satellite
}

local X, Y, Z, list_of_incorrect_sat_indices = trilateration(list_of_satellites, previous_X, previous_Y, previous_Z)
if X then
   print(X, Y, Z)
   if #list_of_incorrect_sat_indices > 0 then
      print("Satellites at the following indices are incorrect: "..table.concat(list_of_incorrect_sat_indices, ","))
   end
else
   print"Not enough satellites"
end

Выход:

111 112 113
Satellites at the following indices are incorrect: 3,5

Answer 2

Такая система трилатерации уже была разработана для другого мода под названием ComputerCraft. Поскольку он, вероятно, не совместим с вашей конкретной проблемой, вам придется модифицировать и адаптировать его логику, но сам алгоритм должен работать.

Вот исходный код

CHANNEL_GPS = 65534

local function trilaterate( A, B, C )
    local a2b = B.vPosition - A.vPosition
    local a2c = C.vPosition - A.vPosition

    if math.abs( a2b:normalize():dot( a2c:normalize() ) ) > 0.999 then
        return nil
    end

    local d = a2b:length()
    local ex = a2b:normalize( )
    local i = ex:dot( a2c )
    local ey = (a2c - (ex * i)):normalize()
    local j = ey:dot( a2c )
    local ez = ex:cross( ey )

    local r1 = A.nDistance
    local r2 = B.nDistance
    local r3 = C.nDistance

    local x = (r1*r1 - r2*r2 + d*d) / (2*d)
    local y = (r1*r1 - r3*r3 - x*x + (x-i)*(x-i) + j*j) / (2*j)

    local result = A.vPosition + (ex * x) + (ey * y)

    local zSquared = r1*r1 - x*x - y*y
    if zSquared > 0 then
        local z = math.sqrt( zSquared )
        local result1 = result + (ez * z)
        local result2 = result - (ez * z)

        local rounded1, rounded2 = result1:round( 0.01 ), result2:round( 0.01 )
        if rounded1.x ~= rounded2.x or rounded1.y ~= rounded2.y or rounded1.z ~= rounded2.z then
            return rounded1, rounded2
        else
            return rounded1
        end
    end
    return result:round( 0.01 )
end

local function narrow( p1, p2, fix )
    local dist1 = math.abs( (p1 - fix.vPosition):length() - fix.nDistance )
    local dist2 = math.abs( (p2 - fix.vPosition):length() - fix.nDistance )

    if math.abs(dist1 - dist2) < 0.01 then
        return p1, p2
    elseif dist1 < dist2 then
        return p1:round( 0.01 )
    else
        return p2:round( 0.01 )
    end
end

function locate( _nTimeout, _bDebug )
    -- Let command computers use their magic fourth-wall-breaking special abilities
    if commands then
        return commands.getBlockPosition()
    end

    -- Find a modem
    local sModemSide = nil
    for n,sSide in ipairs( rs.getSides() ) do
        if peripheral.getType( sSide ) == "modem" and peripheral.call( sSide, "isWireless" ) then   
            sModemSide = sSide
            break
        end
    end

    if sModemSide == nil then
        if _bDebug then
            print( "No wireless modem attached" )
        end
        return nil
    end

    if _bDebug then
        print( "Finding position..." )
    end

    -- Open a channel
    local modem = peripheral.wrap( sModemSide )
    local bCloseChannel = false
    if not modem.isOpen( os.getComputerID() ) then
        modem.open( os.getComputerID() )
        bCloseChannel = true
    end

    -- Send a ping to listening GPS hosts
    modem.transmit( CHANNEL_GPS, os.getComputerID(), "PING" )

    -- Wait for the responses
    local tFixes = {}
    local pos1, pos2 = nil, nil
    local timeout = os.startTimer( _nTimeout or 2 )
    while true do
        local e, p1, p2, p3, p4, p5 = os.pullEvent()
        if e == "modem_message" then
            -- We received a reply from a modem
            local sSide, sChannel, sReplyChannel, tMessage, nDistance = p1, p2, p3, p4, p5
            if sSide == sModemSide and sChannel == os.getComputerID() and sReplyChannel == CHANNEL_GPS and nDistance then
                -- Received the correct message from the correct modem: use it to determine position
                if type(tMessage) == "table" and #tMessage == 3 then
                    local tFix = { vPosition = vector.new( tMessage[1], tMessage[2], tMessage[3] ), nDistance = nDistance }
                    if _bDebug then
                        print( tFix.nDistance.." metres from "..tostring( tFix.vPosition ) )
                    end
                    if tFix.nDistance == 0 then
                        pos1, pos2 = tFix.vPosition, nil
                    else
                        table.insert( tFixes, tFix )
                        if #tFixes >= 3 then
                            if not pos1 then
                                pos1, pos2 = trilaterate( tFixes[1], tFixes[2], tFixes[#tFixes] )
                            else
                                pos1, pos2 = narrow( pos1, pos2, tFixes[#tFixes] )
                            end
                        end
                    end
                    if pos1 and not pos2 then
                        break
                    end
                end
            end

        elseif e == "timer" then
            -- We received a timeout
            local timer = p1
            if timer == timeout then
                break
            end

        end 
    end

    -- Close the channel, if we opened one
    if bCloseChannel then
        modem.close( os.getComputerID() )
    end

    -- Return the response
    if pos1 and pos2 then
        if _bDebug then
            print( "Ambiguous position" )
            print( "Could be "..pos1.x..","..pos1.y..","..pos1.z.." or "..pos2.x..","..pos2.y..","..pos2.z )
        end
        return nil
    elseif pos1 then
        if _bDebug then
            print( "Position is "..pos1.x..","..pos1.y..","..pos1.z )
        end
        return pos1.x, pos1.y, pos1.z
    else
        if _bDebug then
            print( "Could not determine position" )
        end
        return nil
    end
end

Спросите, есть ли у вас какие-либо конкретные вопросы по поводу исходного кода.