瞎猜 htc lighthouse 定位数学模型（一）

作者按：第一次接触htc lighthouse是大约去年年初，公司买了几套做集成开发，当时体验steam vr 几个demo，震惊不已，手柄定位非常准确，几乎感觉不到有延时，下来之后非常好奇它的工作原理，上网找了一些资料，但是没有怎么深入就放下了，直到九月份时候，有一个机会需要与领导一起到台北htc总部谈一项基于vive应用合作开发，当时想，如果不了解lighthouse原理可能不太合适，自己找了很多业余时间理论上推了一下lighthouse定位的数学公式，也找了一些数学大牛帮我review，反正最后数学逻辑上可以自洽，不过到了htc才发现这些都是多虑了，去htc根本没有交流到这种地步：）；现把当时原创瞎猜分享到公众号，文章涉及的内容只是个人业余爱好，数学模型纯属猜测，如有雷同纯属巧合。

htc lighthouse定位可以大致上分成两部分：一是lighthouse 本地光学定位，二是与惯性传感器进行数据融合（平滑和提高输出速率）；本文仅涉及第一部分，数据融合放到下次再猜。（特别备注：内容中有些地方换成了图片是为了更好显示数学符号）。

• 关于lighthouse 工作原理

设备工作原理说明，网上已经有很多，比如这个：http://gizmodo.com/this-is-how-valve-s-amazing-lighthouse-tracking-technol-1705356768 就说的很详细。

• 数学模型
  

• 坐标系定义

定义3个相关的坐标系：大地坐标系（世界坐标系）G，lighthouse光学坐标系C以及手柄（头显）本地坐标系b，O_G，O_C，O_b分别为各自的坐标系的原点。

• XY扫描

定义Lighthouse里面内部激光分别在lighthouse光学坐标系C坐标系下¹XY和²XY面内扫描，圆心分别为（¹x0，¹y0）和（²x0，²y0），又由激光扫描的FOV为120°，60Hz扫描频率，则：角度theta = delta_t * 60 * 120，其中delta_t是接收到同步时间脉冲之后，再接收到激光的时间差。

• 标定

•  标定步骤

需要将手柄，头显放在地面上，2个lighthouse都需要探测到，点击vive上位机操作软件上标定，等待收集一定量数据之后，完成标定。

1. 大地坐标系与本体坐标系

2. lighthouse坐标系与本体坐标系

3. 地面任意放置

4. 正向计算

当求出所有的标定参数之后，已知刚体本体系下坐标求出大地坐标系坐标及姿态就是一件很容易的事情了。

文章转自公众号 ” 应用最优估计”：