First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
95c0b19a26b7c60d78d4a604c12ee25bf9863702
ParkingRobot/detour-information-checklist.md
T

20 KiB
Raw Blame History

Detour 定位接口信息确认（源码答复）

用途：答复控制程序（MultiWheelC / StateEstimation）使用定位结果所必需的接口语义。 依据：本仓库 DetourCore 源码（Location.cs、TightCoupler.cs、Frame.cs、LidarOdometry.cs、LidarMap.cs、WebAPI.cs、G.cs）。 仓库内没有名为 getCartLocation() 的函数；控制侧该调用对应 Detour 的两类对外位姿出口。下文按字段对齐后统一说明。

对外位姿出口（先对齐接口）

控制程序看到的 x / y / theta / tick / l_step，来自下面之一（或对它们的封装）。字段名在源码里是 th，不是 theta。

出口 入口 字段 tick 的真实来源
HTTP GET /getPos CartLocation.FormatPosition() JSON：x, y, th, l_step, tick，异常时多 error tick = CartLocation.st_time（观测时刻）
共享对象 DetourPos{shareObjectTag} TightCoupler.CommitLocation 每次提交后 Post 二进制：x, y, th, tick, l_step, error tick = DateTime.Now.Ticks（提交/推送时刻）

默认 HTTP 端口：Configuration.conf.guru.DetourPort，默认 4321。 shareObjectTag 必须与控制侧 Clumsy 的 soTag 一致。

两种出口的 x/y 都已经过：

x_out = 内部x / guru.inputScale + guru.biasX
y_out = 内部y / guru.inputScale + guru.biasY

默认 inputScale = 1、biasX = biasY = 0，因此默认单位就是内部单位（毫米）。 th / l_step 不做缩放。

1. getCartLocation() 字段定义

1.1 各字段含义

字段 源码名 含义
x CartLocation.x 车体原点在当前地图坐标系中的 X。已从传感器坐标系经安装外参反变换到车体中心（TightCoupler.CommitLocation 里 ReverseTransform(传感器位姿, 组件安装位姿)）。
y CartLocation.y 同上，Y。
theta CartLocation.th 车体航向。0° 朝向地图 +X，逆时针为正（内部用 cos(th) / sin(th) 画朝向）。
tick 见上表 不是统一的一种时间。HTTP 是观测时间 st_time；DObject 是提交瞬间的 DateTime.Now.Ticks。控制侧必须先确认自己走的是哪条通道。
l_step Frame.l_step 到已标注（labeled）关键帧的图上步数，用来表达“离可信锚点有多远”，不是优化迭代次数，也不是匹配分数。注释原文：steps to labeled keyframe。

1.2 单位、正方向、坐标系

  • 内部单位：x、y 为毫米。UI 直接按 mm 显示。
  • 对外单位：默认仍是毫米；只有改了 guru.inputScale（例如设为 1000 输出米）才会变。
  • 角度单位：度。
  • 坐标系：当前加载的 SLAM 地图坐标系。原点由建图/标注关键帧决定，不是 GNSS 或车体启动点。
  • 车体姿态：输出的是车体原点，不是雷达原点。雷达/相机安装位姿在 layout.components 的 x,y,th 里。
  • 右手平面：+X 为航向 0，+Y 为航向 +90°。

1.3 theta 取值范围

不是严格的 [-180°, 180°]。

LessMath.normalizeTh 只在绝对值 ≥ 360 时按 360° 回绕，(-360, 360) 内原样返回。因此对外可能看到 200、-270 这类值。控制程序若假设 [-180, 180] 或 [0, 360)，应自己归一化。内部比较角度用 LessMath.thDiff，会按最短弧处理。

1.4 一次调用的字段是否同一帧

是。 一次读取对应同一个 CartLocation 对象上的 x/y/th/l_step/st_time。 getPos 先用当前 latest 判断 Timeout/Unstable，再序列化；若这两步之间恰好发生新提交，状态字和位姿可能差一帧，但单次 JSON 里的数值字段仍来自同一个对象。

2. tick 的准确含义（优先）

2.1 它是什么时刻

分通道：

HTTP /getPos 的 tick = st_time

  • st_time 在 Frame 上的注释是：传感器数据到达 Detour 时由 G.watch 打的时间，不是激光头硬件曝光时刻，也不是 HTTP 返回时刻。
  • 激光采集线程会按扫描周期做轻微平滑，并加上 time_bias_ms。
  • 里程计把 frame.st_time 原样交给 TightCoupler.CommitLocation，再写入 CartLocation.st_time。
  • 因此：tick ≈ 该位姿所对应的那帧观测到达时刻。SLAM 计算发生在这之后，接口返回更晚。

DObject DetourPos 的 tick = DateTime.Now.Ticks

  • 这是 TightCoupler 提交并推送的本地时刻，100 纳秒为单位，从公元 1 年 1 月 1 日起算（.NET DateTime.Ticks）。
  • 它不是传感器采集时刻，也不是 Unix epoch。
  • 受本机本地时钟、时区、校时影响，必要时可能回跳。

2.2 单位、频率、单调、回绕

项目 HTTP tick（st_time） DObject tick（DateTime.Now.Ticks）
单位 毫秒 100 ns（1 ms = 10000）
时钟 进程启动时锚定一次 UTC，之后用 Stopwatch 单调累加：ElapsedTicks*1000/Frequency + stMillis 本机本地 DateTime.Now
更新频率 随传感器/融合提交，通常接近雷达帧率（常见 10–20 Hz，取决于设备 timeBudget） 每次 CommitLocation 成功推一次
单调 进程内单调递增（不受事后改系统时间影响） 一般递增；改系统时间或 DST 可能回跳
回绕 long 毫秒，实际不会回绕 long ticks，实际不会回绕
进程重启 重新锚定当前 UTC，不会从 0 开始，但与重启前不保证连续 继续跟本机时钟，与进程无关

G.watch.TimeStampMillis 不是 Unix 毫秒（1970），而是从公元 1 年起算的 UTC 毫秒，再加上启动后的单调流逝。

2.3 多车能否用 tick 对齐

不能当作已同步的多车时钟。

  • 各车各自启动 G.watch，没有 PTP/NTP 协议，也没有跨车时间服务。
  • HTTP tick：若各车系统 UTC 在启动时大致同步，数值会接近“同一套绝对时间”，但仍有启动锚定误差和各车处理延迟，不能直接当多车位姿对齐的主时钟。
  • DObject tick：本地时间 ticks，跨时区会直接错开，更不适合多车对齐。
  • 进程重启、暂停（G.paused）、提交失败都会造成时间空洞。

控制侧建议：单车内部用 tick 做延迟估计和短时预测；多车对齐应使用外部统一时钟，或只把 Detour tick 当相对时间。

3. 定位延迟与更新方式

3.1 输出位姿比真实运动滞后多少

源码没有标定“官方延迟 xx ms”。能确定的是延迟结构：

真实运动 → 雷达扫描/到达(st_time) → 里程计配准 → TightCoupler 融合 → 写入 CartLocation.latest → 接口读到缓存

经验上由源码阈值约束：

  • 激光一圈通常几十毫秒（Lidar2DStat.timeBudget）。
  • TightCoupler 常用窗口 TCtimeWndSz = 150 ms，上限 TCtimeWndLimit = 700 ms。
  • LidarOdometry 把 reg_ms > 200 记为 bad perf。
  • /getPos 若 now - st_time > 500 ms 打 Timeout。

因此正常运行时，从观测到可读位姿大约是 1 帧雷达周期 + 配准/融合，常见几十到两百毫秒；超过 500 ms 会被 HTTP 接口判超时。 这是实现上的门槛，不是出厂标定值。

3.2 getCartLocation() 是否返回最近一次缓存

是。

  • CartLocation.latest 是全局缓存。
  • 只有 TightCoupler.CommitLocation 成功后才会替换（取 history 中 st_time 最新的一帧）。
  • /getPos 只读缓存，不触发新的 SLAM 计算。
  • 配准失败、CommitLocation 返回 null（bad variance / bad trace）、或 G.paused 时，缓存停在上一帧。

3.3 原地自转时频率/延迟是否明显变化

轮询频率不变，有效更新和延迟会变差。

  • 接口仍按调用方频率读同一缓存。
  • 原地旋转时二维扫描重叠变差，帧间分、相位锁分容易掉，allowCommit 更常失败，缓存更容易停住。
  • 配准变慢时单帧 reg_ms 上升；失败后 l_step 被加大（见第 4 节），看起来像“转的时候定位变钝”。
  • 源码没有“自转专用降频”。

控制侧：自转短时预测应假设有效位姿更新可能变稀、变老，不要假设 tick 仍按雷达周期前进。

4. l_step 的含义（优先）

4.1 它是什么

定位质量的图距离，不是优化迭代次数，也不是匹配状态枚举。

定义：当前位姿/关键帧沿约束图走到已标注关键帧还要几步。

  • 0：本身就是标注帧（labeledXY / labeledTh），或刚被标成锚点。
  • 1：刚和地图/标注帧配准成功（LidarMap 成功后会把 compared.l_step = 1）。
  • 正常递推：新关键帧 l_step = 旧关键帧.l_step + lstepInc。
  • TightCoupler 提交时：有参考关键帧则 reference.l_step + 1；没有则 latest.l_step + 3。
  • 9999：未定位、手动设位、或与标注图断开。这是 Frame / 初始 CartLocation 的默认值。

越小越好、越可信。 AIImplementationNote.md 里“越小越不确定”与源码相反，不要采用。

内部用途包括：

  • TightCoupler 边权：l_step > 10 → 1，> 5 → 2，> 2 → 3，否则 4；标注帧权 1000。
  • 图优化用 1/(l_step+0.01) 当权。
  • l_step 大时 LidarMap 更容易走全局配准（forceGRegStep、GregThresK * l_step）。
  • l_step > 1000 且孤立的关键帧可被删掉。

源码 TODO 也写过：希望将来“去掉 l_step，改用误差圆”。当前对外接口仍是这个整数。

4.2 为什么原地自转会从 2～4 升到几十、上百

自转本身不会改公式，但会让 lstepInc 连续被惩罚，再在切关键帧时一次性加到 l_step 上。

LidarOdometry 里常见加项：

条件 lstepInc 增量
帧间隔过大 +3～+20
时间落后 > 1 s 可到 +1000 量级并重启局部图
有效点太少 +3
掩膜后点过少 +10
帧间序贯配准失败 +5
局部里程计分过低 +15
历史分偏低 +1～+15
局部图重启 / 相位锁差 +2
上一帧能提交、这一帧不能 +100
离开 hard 区域 默认再 +20

原地自转时典型连锁是：旋转导致重叠变差 → 分数掉 → lstepInc 累加 → 因“新点变多 / 超时 / restart”切关键帧 → 新 l_step = 旧值 + lstepInc。 若此时地图匹配也失败，TightCoupler 按 latest.l_step + 3 继续推高。 所以 2～4 很快可以变成几十，失败恢复前看到上百是符合实现的。

匹配一旦重新成功，关键帧会被改回 l_step = 1，输出也会掉下来。

4.3 有没有官方阈值

没有写给控制程序的官方阈值表。 下面是源码内部实际在用的分档，可作控制侧初值，现场仍要按地图和雷达标定。

l_step 源码含义 给控制程序的建议
0 标注锚点 最可信
1～2 刚贴上地图 / 离锚点很近 正常，接受
3～5 TightCoupler 已降权 可用，开始警惕跳变
6～10 权更低；地图更倾向全局配准 退化，降低信任 / 收紧预测
11～98 最低权；曾从不稳定区回到 2 时会打工作区快照 明显退化，不宜做精细控制
≥ 99 TightCoupler 允许关键帧被推得更狠 按不可靠处理
≥ 1000 孤立帧可删 基本断开
9999 未定位 / 手动设位 / 断开 不可用，应安全停止或等待重定位

Detour 自己判断“车是否静止可做全局重定位”时用：l_step > 2 且 TightCoupler 速度估计很小。这是内部策略，不是对外状态机。

5. 坐标跳变与重定位行为（优先）

5.1 重定位、回环、失败恢复后，x/y/theta 会不会永久跳变

会。跳变是设计行为，不是毛刺。

会改当前输出的情况：

  1. 地图配准成功：LidarMap 把当前关键帧改到匹配位姿，并 l_step = 1，再交给 TightCoupler。下一帧 CartLocation 跟着新参考走，表现为一次台阶。
  2. 全局重定位（/relocalize 或 UI Relocalize）：定位器进入 relocalizing，按全图关键帧搜索（source = 9）。第一次更好的匹配会 TightCoupler.Reset，位姿被拉到地图上，通常是大幅度永久跳变。
  3. 回环 + GraphOptimizer：关键帧 x/y/th 被就地改写。当前参考帧若被挪动，后续融合位姿跟着变。优化有动量平滑，但仍可能出现肉眼可见的台阶。
  4. 手动 /setLocation：直接改 CartLocation.latest 并重置融合窗口。
  5. 匹配长期失败后突然恢复：从里程计漂过的位置一下子贴回地图，跳变幅度等于累计漂移。

没有“只在内部跳、对外插值抹平”的保证。控制程序必须自己做跳变连续化或拒绝。

5.2 跳变后还在原来的地图坐标系吗

同一张已加载地图内：是。 跳的是车在这张图里的估计，不是换了一套轴。

会换坐标系的只有：换图（/loadMap）、Remapper（GNSS/外参映射）、或手动把车标到另一个锚点。这些不是普通重定位。

5.3 有没有“正在重定位 / 定位丢失 / 地图坐标调整”标志

定位结果包里没有这些标志。

内部有、但不随 getPos / DetourPos 下发：

内部量 作用 是否对外
Locator.relocalizing / relocalized 地图层正在/已经全局搜 否
G.IsSettingPosition 正在手动设位 否（/getStat 的 globalStat 里能看到）
G.paused 定位暂停 否（同上，或调 /pause /resume）
CartLocation.unstable 本帧掩膜过狠、场景不稳定 仅 HTTP：error = "Unstable"。DObject 当前推送的 error 被写成空串
l_step 升到很大 / 9999 实际的“丢了” 是，但这是间接指标
图优化改关键帧 “地图坐标调整” 无单独标志

HTTP /getPos 仅有的显式错误：

  • "Timeout"：st_time 已超过 500 ms
  • "Unstable"：latest.unstable == true

没有 "Relocalizing"、"Lost"、"MapAdjusted"。 /getStat 可拉到各模块 StatusMember（雷达间隔、里程计状态、TC 状态等），但不是每帧位姿附属字段，也不适合当硬实时互锁。

控制侧应自己构造状态：

  • 疑似丢失：error 为 Timeout/Unstable，或 l_step ≥ 99，或 tick 长期不涨。
  • 疑似重定位/回环跳变：相邻两帧 x/y/th 突变，同时 l_step 突然掉回 1～2。
  • 地图在拧：跳变较缓、持续多帧，且 l_step 并不爆掉。

6. 可用的定位质量接口（优先）

6.1 除 l_step 外还能拿到什么

位姿包几乎只有 l_step + HTTP 的 error。

信息 有没有 说明
匹配得分 对控制程序：无 在 LidarOdometry / LidarMap 内部（score、phaselocker_score），不下发
协方差 对控制程序：无 Frame.errXX/errXY/errYY 已预留，未填进 /getPos 或 DetourLocation
置信度 间接 就是 l_step；源码 TODO 想换成误差圆，尚未做
定位状态 很弱 HTTP：Timeout / Unstable；DObject：error 目前恒为空
错误码 无枚举 只有上述字符串
系统状态 有，但是慢接口 GET /getStat → 布局/里程计/定位器/TC/GO/G.paused/G.IsSettingPosition

因此控制程序不能指望每帧拿到匹配分或协方差。

6.2 Detour 实际用什么条件接受一帧（可当作官方内部规则）

源码里“接受并提交”的条件是分层的，没有单独的对外规范文档：

  1. 里程计层（LidarOdometry.updateLocation）
    • 分数过低会把 strength 压下去，甚至不把 reference 挂上。
    • 掩膜过狠 → unstable = true。
    • 点太少、序贯/局部配准失败 → 不切健康关键帧，并加大 lstepInc。
  2. 融合层（TightCoupler.CommitLocation）
    • 传感器处于 bad variance / bad trace → 直接丢弃，返回 null。
    • 丢弃后该源会被禁一段时间（约 1～2 s）。
  3. 地图层（LidarMap）
    • result.score < ScoreThres 丢弃。
    • 相对已有位姿的 xy/th 偏差超过按 l_step 放大的门限则丢弃（防止乱跳）。
    • 落到无效区域丢弃。
  4. HTTP 出口
    • 缓存超过 500 ms → Timeout。
    • unstable → Unstable。

6.3 建议控制程序如何接受/拒绝一帧

源码没有写给 MultiWheelC 的官方判据。按上面的内部规则，建议：

接受（正常闭环）

  • HTTP：无 error（DObject：至少 tick 在前进）。
  • l_step ≤ 5。
  • tick 新鲜（HTTP：now - tick < 300 ms 较稳妥；DObject：换算成 ms 后同样看提交间隔）。
  • 相对上一接受帧：位移/转角不超过本底盘短周期能达到的上限。

降级（短时预测、降低增益、禁止精细对位）

  • 6 ≤ l_step ≤ 10，或偶发 Timeout 后立刻恢复。
  • 原地自转期间 l_step 爬升但 tick 仍在更新。

拒绝并安全停止 / 等待

  • error == "Timeout" 持续，或 error == "Unstable"。
  • l_step ≥ 99（含 9999）。
  • 单帧出现与运动学不符的永久台阶（尤其伴随 l_step 从很大突然回到 1）：先当重定位跳变，做连续化或刹停，不要直接当编码器。

不要做的事

  • 不要用 l_step 当 ICP 迭代次数或“正在计算中”。
  • 不要假设 theta ∈ [-180, 180]。
  • 不要用 tick 做多车时间同步主时钟。
  • 不要假设 DObject 的 error 会带 Timeout/Unstable（当前实现是空的）。

对 StateEstimation 的直接含义

清单里最优先的 2 / 4 / 5 / 6，对应控制侧应这样定：

  1. 时间同步

    • 先确认 getCartLocation 走 HTTP 还是 DetourPos。两条通道的 tick 单位和语义都不同。
    • 单车：用 tick 估延迟、做自转短时预测。
    • 多车：另选同步时钟。

  2. 自转短时预测

    • 自转时有效更新可能变稀，l_step 会从个位数爬到几十上百。
    • 这是质量变差，不是接口卡死。预测窗口应随 tick 变老、l_step 变大而缩短。

  3. 跳变连续化

    • 重定位、回环、失败恢复都会造成同地图下的永久台阶。
    • 没有“正在改图”标志；用位姿差分 + l_step 回落来识别。

  4. 安全停止

    • 硬条件：Timeout / Unstable / l_step ≥ 99 / tick 停更。
    • l_step 建议按 ≤5 正常、6–10 退化、>10 不可用于精细控制。

源码锚点

主题 位置
对外 JSON / Timeout / Unstable DetourCore/Location.cs：ConstructRet、FormatPosition
HTTP 路由 DetourCore/WebAPI.cs：/getPos、/setLocation、/relocalize、/getStat
DObject 推送与 tick=DateTime.Now.Ticks DetourCore/Algorithms/TightCoupler.cs：DetourLocation、CommitLocation
l_step 定义 DetourCore/Types/Frame.cs
车体中心变换、提交缓存 TightCoupler.CommitLocation
时钟 DetourCore/G.cs：DetourWatch.TimeStampMillis
角度回绕 DetourCore/LessMath.cs：normalizeTh
自转时 l_step 被加大 DetourCore/Algorithms/LidarOdometry.cs（lstepInc）
匹配成功后的位姿跳变 DetourCore/LocatorTypes/LidarMap.cs（loop / relocalize）
回环拧图 DetourCore/Algorithms/GraphOptimizer.cs
全局重定位入口 DetourCore/DetourLib.cs：Relocalize()
单位缩放 DetourCore/Configuration.cs：GuruOptions.inputScale / biasX / biasY

修订记录

  • 2026-08-24：按当前 Detour 源码逐条答复原《Detour 信息确认清单》。
Reference in New Issue View Git Blame Copy Permalink