模拟 3D 空间中的简单三边测量算法答案

【问题标题】：Simple trilateration algorithm in simulated 3D space模拟 3D 空间中的简单三边测量算法
【发布时间】：2019-01-19 16:14:52
【问题描述】：

上下文：我正在为 OpenComputers 添加的移动计算机实现导航系统，这是一个 Minecraft 模组。对于那些不熟悉这个模组的人来说，它基本上添加了各种 Lua 可编程、可升级的计算机，包括移动计算机——即机器人、无人机和平板电脑。尝试对机器人和无人机进行编程以执行自主任务时经常出现的众多挑战之一是确保它们始终知道自己的坐标。

最简单的解决方案是使用导航升级，它正是这样做的 - 为计算机提供其相对于制作地图中心的精确坐标。然而，它有两个主要缺点 - 它占用了 II 级升级槽，这可不是小事，而且仅限于地图区域。后者或多或少是可以接受的，但仍然使这种导航方法在某些用例中不可用。

另一种解决方案是让计算机记住一次它们的坐标，然后跟踪它们的运动，但这也有许多潜在的警告 - 你必须通过自定义子程序控制所有运动或使用黑客来拦截组件调用，每次都必须手动输入坐标，您就无法移动计算机，无人机存在一些精度误差，这对平板电脑根本不起作用。

第三种方法——我正在研究的方法——类似于现实生活中的 GPS。它基于这样一个事实，即计算机可以使用无线网卡进行升级，以便能够在 400 块的相当大的距离内相互发送消息，并且随着消息本身它们接收到精确的距离（浮点数，以块为单位） ) 在发送者和接收者之间。如果我们将一些固定计算机指定为不断广播其位置的“卫星”，我们可以使移动计算机能够使用来自 4 颗以上卫星的信息进行三边测量。

这种方法具有可扩展性（您可以不断向网络中添加更多卫星以扩大其覆盖范围），不会占用仅用于导航目的的额外升级槽（因为许多移动计算机已经升级了无线网卡）并且精确，这使它比其他两种方法具有明显的优势。但是，它需要一些令人惊讶的复杂计算，这就是我遇到的困难。

问题：我需要找到一个三边测量算法（最好附带一个代码示例），它可以让任何移动计算机在知道指定“卫星”的坐标和到它们的距离。我们假设所有的计算机和卫星都配备了 Tier II 无线网卡（即它们可以在 400 个块的总范围内相互发送消息，并以 float32 数字允许的精度知道发送者与自身之间的距离）。该解决方案将使用纯 Lua 编码，无需访问任何第三方服务，因此像 Mathematica 这样的数据包是不行的。目前我正在押注某种拟合方法，尽管我不知道如何实现，以及它能否适应范围内某些卫星广播错误位置的可能性。

在最基本的层面上，我们可以假设有 4 颗卫星不断且正确地广播它们的位置，彼此相距适中的距离，并且不位于单个 2D 平面上。理想情况下，算法应该能够适应一些可选条件 - 请参阅下面的部分。

奖励积分：

使算法足够小以适应无人机的 2KB 内存（假设 UTF8 编码）。但是，它应该占用更少的空间，以便主程序也可以容纳。越小越好。
制定一种算法，允许卫星彼此非常接近并具有非整数坐标（以允许用一个不断移动的机器人或无人机替换多个固定卫星，或使移动数据终端本身随其移动从一颗卫星上进行测量）。
假设已经可以确定位置，制定允许存在少于 4 颗卫星的算法 - 例如，如果所讨论的移动数据终端是机器人，并且除了一个可能的位置之外的所有位置都低于或高于允许的高度块的范围（y255）。如果有三颗卫星位于 y=255 的高度，则这种设置是可能的。
制定一种算法，可以抵抗某些卫星广播的位置稍有错误（设置中的一个小错误）。鉴于存在足够正确的测量值，该算法应该推断出正确的位置或完全抛出错误。理想情况下，它还可以记录“关闭”卫星的位置。
制定一种算法，该算法可防止同时存在两组或多组卫星在不同坐标系中正确广播其位置（设置中的一个重大错误）。每个网络都有一个（假设是唯一的）标识符，允许区分由不同玩家（或者，好吧，只有一个）独立设置的不同网络。但是，如果他们不费心正确设置标识符，不同的信号可能会混淆，从而使移动数据终端感到困惑。因此，抵抗算法应该能够检测到这种情况，或者直接抛出错误或区分不同的网络（然后可以对其进行微调以适应特定应用程序的目的 - 即拒绝加载，选择最近的网络，选择最大的网络、提示用户或控制服务器等）。

我的尝试：除了尝试自己解决问题外，我还尝试在互联网上查找合适的解决方案。但是，我能找到的解决方案都不适合这项任务。

我通过谷歌搜索“三边测量算法”发现的大部分内容都是处理现实生活中的 GPS 系统 - 也就是说，仅使用 2 个坐标，强烈考虑错误并且通常没有提供足够的精度。李>
相反，有些是纯数学的，建议建立一系列方程来找到球体的交点。可悲的是，据我较弱的数学背景让我理解，这种方法没有考虑浮点数的精度误差 - 圆圈不相当相交，点不是相当在相同的位置，所以方程没有解。
有些似乎解释了解决方案，但涉及很多我无法理解的复杂数学，并且没有包含确切的算法或至少一个代码示例。
At least one 使用了 Mathematica 等外部数据包，但在这种情况下又不可用。

如果我留下了一些不清楚的重点，请发表评论，以便我改进问题。提前致谢！

【问题讨论】：

当有两组卫星（每组都有自己的坐标系）时，预期的输出是什么？程序应该只输出你自己组中的坐标吗？还是应该输出如下内容：“Group A: (xa,ya,za); GroupB: (xb,yb,zb)”？不同坐标系的轴是否平行？ GroupA 中的 x 轴是否可以与 GroupB 中的 x 轴平行，但方向相反？
感谢您的提问，@EgorSkriptunoff！ 1）如果导航系统设置正确，那么移动计算机应该忽略所有他们不知道如何使用的网络。所以是的，程序应该只在自己的组中输出坐标。如果它 not 设置正确 - 即。检测许多组并且不知道哪个是“它自己的” - 然后，理想情况下，它应该检测哪些卫星属于哪个组，并将其余的留给实现（默认情况下只是抛出一个错误）。 2) 是的，不同坐标系的轴都是同向对齐的，并且相互平行。

标签： algorithm lua minecraft trilateration opencomputers

【解决方案1】：

这样的三边测量系统已经为另一个名为 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

询问您是否对源代码有任何具体问题。

【讨论】：

谢谢！这段代码看起来合法且有效，但我很可能会选择@EgorSkriptunoff 的答案，因为它更详细、更高级、提供更多功能并且已经适用于 OC。尽管如此，您提供的代码is bundled with 是官方的ComputerCraft 发行版，并且经常被很好地使用，这应该意味着它非常好和高效。不过，您可能应该提供指向源的链接。 :)

【解决方案2】：

函数trilateration 需要卫星列表（它们的坐标和与移动计算机的距离）和移动计算机以前的坐标。
仅收集您自己组中的卫星，排除所有其他组中的卫星。
您的某些卫星可能会发送不正确的数据，这没关系。

如果没有足够的卫星可访问，该函数将返回nil，因为它无法确定当前位置。
否则，该函数返回移动数据终端的当前坐标和卫星索引列表被指责为不正确。
如果有歧义，新位置将被选为最接近移动数据终端先前位置的位置。
输出坐标为整数，Y坐标限制在0..255范围内

正确的三边测量应满足以下条件：

(number_of_correct_satellites) 必须 >= 3
(number_of_correct_satellites) 必须 >= 4 如果至少存在一颗不正确的卫星
(number_of_correct_satellites) 必须 > (number_of_incorrect_satellites)

识别不正确的卫星是昂贵的 CPU 操作。
一旦卫星被识别为不正确，请将其存储在某个黑名单中，并将其从所有未来的计算中排除。

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

【讨论】：

谢谢！我目前无法检查此代码的实际运行情况（Java 在玩弄操作系统后会起作用），但它看起来是合法的。我会尽快尝试并报告结果。据我所知，它还可以检测和排除不正确的卫星，但是通过暴力破解来做到这一点，这意味着它的执行时间将呈指数增长，并且如果卫星列表足够大，它将挂断整个计算机“太长而没有屈服”错误。但是，对于特定的应用程序，这个问题可以很容易地解决，并且超出了这个问题的范围。 :)
@DeFazer - 如果一次出现一个不正确的卫星，那么检测它们会非常快。是的，因为 exp 增长。
@DeFazer - 你在现实世界中测试过我的代码吗？我想听一些批评:-)
@EgorSkriptunoff：还没有，抱歉！我目前在启动 Minecraft 时遇到问题（似乎是 this）。很尴尬，这么快就收到了一个看似不错的答案，却无法验证和接受。我希望我能尽快找到解决方案，然后能够尝试您的代码。 ^^
你能分享一下这个算法使用的方法吗？理想情况下，算法内部的一些数学 URL 会很好。对这里发生的事情有一些解释会很棒：local e = exp(err+err); e = (e-1)/(e+1)/(d+1); DX = DX + dxe; DY = DY + dye; DZ = DZ + dz*e，或者至少要有带解释的 URL。