Algorithm 模拟三维空间中的简单三边测量算法
上下文:我正在为OpenComputers(一种Minecraft mod)添加的移动计算机实施导航系统。对于那些不熟悉国防部的人来说,它基本上增加了各种Lua可编程、可升级的计算机,包括移动计算机——即机器人、无人机和平板电脑。当试图为机器人和无人机编程以执行自主任务时,经常出现的许多挑战之一是确保它们始终知道自己的坐标 最简单的解决方案是使用导航升级,导航升级正是这样做的——为计算机提供其相对于地图中心的精确坐标。然而,它有两个主要的缺点-它占用了一个Tier II升级槽,这不是一件小事,而且仅限于地图区域。后者或多或少是可以接受的,但仍使此导航方法在某些使用情况下不可用 另一个解决方案是让计算机记住它们的坐标一次,然后跟踪它们的移动,但这也有一些潜在的警告——你必须通过自定义子程序控制所有移动,或者使用黑客拦截组件调用,每次移动电脑都必须手动输入坐标,无人机会出现一些精度错误,平板电脑根本无法做到这一点 第三种方法——我正在研究的方法——类似于现实生活中的GPS。这是基于这样一个事实:计算机可以通过无线网卡进行升级,以便能够在400个块的相当大的距离内相互发送消息,并且随着消息本身,它们接收发送方和接收方之间的精确距离(以块为单位的浮点数)。如果我们将一些固定计算机指定为“卫星”,不断广播它们的位置,我们就可以使一台移动计算机能够使用来自4颗以上卫星的信息对其准确位置进行三边测量 这种方法具有可扩展性(您可以不断向网络添加更多卫星以扩大其覆盖范围)、不占用额外的升级槽(因为许多移动计算机已经使用无线网卡进行了升级)和精确性,这使得它比其他两种方法具有明显的优势。然而,它需要一些令人惊讶的复杂计算,这就是我陷入困境的地方 问题:我需要找到一种三边测量算法(最好附带一个代码示例),该算法允许任何移动计算机计算其位置(误差范围约为0.25个区块),知道指定“卫星”的坐标和到它们的距离。我们假设所有计算机和卫星都配备了第二层无线网卡(即,它们可以在400个数据块的总范围内相互发送消息,并以32个数字所允许的精度知道发送者与自身之间的距离)。解决方案将使用纯Lua编码,而不访问任何第三方服务,因此Mathematica之类的数据包是不允许的。目前,我正押注于某种合适的方法,尽管我不知道如何实现一种方法,也不知道它是否能够很好地适应某些距离内的卫星广播错误位置的可能性 在最基本的层面上,我们可以假设有4颗卫星持续正确地广播其位置,彼此相距中等距离,不位于单个2D平面上。算法在理想情况下应该能够适应一些可选条件-参见下面的部分 奖励积分用于:Algorithm 模拟三维空间中的简单三边测量算法,algorithm,lua,minecraft,trilateration,opencomputers,Algorithm,Lua,Minecraft,Trilateration,Opencomputers,上下文:我正在为OpenComputers(一种Minecraft mod)添加的移动计算机实施导航系统。对于那些不熟悉国防部的人来说,它基本上增加了各种Lua可编程、可升级的计算机,包括移动计算机——即机器人、无人机和平板电脑。当试图为机器人和无人机编程以执行自主任务时,经常出现的许多挑战之一是确保它们始终知道自己的坐标 最简单的解决方案是使用导航升级,导航升级正是这样做的——为计算机提供其相对于地图中心的精确坐标。然而,它有两个主要的缺点-它占用了一个Tier II升级槽,这不是一件小事,而
- 使算法足够小,可以放入无人机的2KB内存(假设UTF8编码)。不过,它所占用的空间应该比这个小得多,这样一个主程序也可以安装。越小越好
- 制定一种算法,允许卫星彼此非常接近,并且具有非整数坐标(允许用一个不断移动的机器人或无人机替换多个固定卫星,或者允许移动计算机本身在从单个卫星进行测量时移动)
- 制定一个允许存在少于4颗卫星的算法,假设位置已经可以确定——例如,如果所讨论的移动计算机是一个机器人,并且除了一个可能的位置之外,所有其他位置都低于或高于允许的区块高度范围(y255)。如果有三颗卫星位于y=255的高度,则可以进行这种设置
- 制定一种算法,该算法能够抵抗某些卫星广播的轻微错误位置(设置中的一个小错误)。如果存在足够多的正确测量值,算法应该推断出正确的位置,或者干脆抛出一个误差。理想情况下,它还可以记录“关闭”卫星的位置
- 制定一种算法,防止两组或多组卫星同时在不同坐标系中正确广播其位置(设置中的一个重大错误)。每个网络都有一个(据说是唯一的)标识器,可以区分不同玩家(或者,仅仅一个)独立设置的不同网络。然而,如果他们不费心去正确设置识别器,不同的信号可能会混淆,混淆移动计算机。因此,抵抗算法应该能够检测到这种情况,并直接抛出错误或区分不同的网络(然后可以对其进行微调,以适应特定应用程序的目的,即拒绝加载、选择最近的网络、选择最大的网络、提示用户或控制服务器等)
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
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