T265 接口

本节介绍

t265.py 读取 Intel RealSense T265 视觉里程计的位姿和速度，做坐标系转换后提供给状态机和 PID 使用。包含模拟降级模式，无硬件时也能跑通完整流程。

学习目标

1. 知道 T265 是什么，为什么不需要 GPS
2. 理解 T265 坐标系到项目坐标系的转换逻辑
3. 能说出 get_position() / get_velocity() / get_tracking_confidence() 的返回值
4. 知道 DRY_RUN 模拟模式如何工作
5. 了解低通滤波和校准偏移的作用

---

T265 是什么

T265 是 Intel 的视觉里程计（Tracking Camera），内置两个鱼眼摄像头和 IMU，通过 VIO（视觉惯性里程计）算法实时输出 6 自由度位姿。不需要 GPS，适合室内飞行。

特性	值
输出频率	200Hz（原始），约 30Hz（滤波后有效）
置信度	0-3，≥2 才认为定位有效
坐标系	T265 原始坐标系（需转换到项目坐标系）
接口	USB，pyrealsense2 库

坐标系转换

T265 坐标系与项目使用的航空坐标系不同，需要矩阵变换：

T265 坐标系:  X=右, Y=上, Z=后
项目坐标系:   X=前, Y=左, Z=上

转换矩阵 H_aeroRef_T265Ref:
  [1  0  0  0]
  [0  0  1  0]
  [0 -1  0  0]
  [0  0  0  1]

转换后：

项目坐标	T265 原始值	说明
X (前)	-T265.Z	前方为负 Z
Y (左)	-T265.X	左方为负 X
Z (上)	+T265.Y	上方为正 Y

---

API 参考

class t265_class

T265 视觉里程计接口，含坐标系转换和模拟降级

生命周期方法

.start() → bool

启动 T265 管线和数据采集线程（daemon）。成功返回 True，失败返回 False。DRY_RUN 模式跳过硬件初始化。

返回 bool — True = 启动成功

.stop() → None

停止数据线程，关闭 pipeline。join 超时 2 秒。

.is_running() → bool

返回 T265 是否在运行。Lprotocol 的 T265 发送线程用它做心跳检测。

数据读取方法

位置

.get_position() → np.ndarray

返回校准后的位置 [x, y, z]（米）。已做低通滤波和 calibration_offset 校准。线程安全。

返回 np.ndarray — shape (3,)，单位米

.get_raw_position() → np.ndarray

诊断用：低通滤波前的原始位置。不用于飞控闭环。

速度

.get_velocity() → np.ndarray

返回速度 [vx, vy, vz]（m/s）。已做低通滤波。线程安全。

返回 np.ndarray — shape (3,)，单位 m/s

姿态

.get_orientation() → np.ndarray

返回姿态 [roll, pitch, yaw_err]（弧度）。yaw_err 是相对初始 Yaw 的偏差。

.get_yaw_deg_x100() → int

返回 Yaw 偏差 × 100 的整数值（度）。供 Lprotocol 发送给飞控。

置信度与诊断

.get_tracking_confidence() → int

返回 T265 跟踪置信度 (0-3)。Mission_GPT 用它做启动门禁和飞行安全检测。

置信度	含义	行为
0-1	低置信度	位置冻结，不更新
2-3	有效	正常使用

.get_pose_age_s(now: float = None) → float

返回最后一个 Pose 帧的年龄（秒）。从未收到时返回 inf。用于检测 T265 数据流中断。

.get_pose_frame_count() → int

返回自启动以来收到的 Pose 帧总数。用于诊断数据流是否正常。

原始 IMU

.get_raw_imu() → np.ndarray

返回 T265 内置加速度计/陀螺仪原始数据 [ax,ay,az,gx,gy,gz]。不经过融合，独立于凌霄 IMU。用于诊断电机振动特征。

校准方法

位置校准

.autoset() → bool

采样 10 帧位置取均值作为校准偏移。起飞前调用，将当前停留点设为零点。同时重置初始 Yaw。

.apply_position_continuity_correction(delta_xyz: array-like) → np.ndarray

修正位置跳变。将 delta 加入 calibration_offset，不影响 T265 原始坐标系。用于 T265 复位后的无缝接续。

参数	类型	说明
delta_xyz	array-like	三维位移向量，必须有限

低通滤波

.low_pass_filter(new_val: float, prev_val: float, alpha: float = 0.15) → float

一阶低通滤波。alpha=0.15 意味着新值权重 15%，平滑但有一定延迟。

参数	类型	默认值	说明
new_val	float	—	新测量值
prev_val	float	—	上次滤波结果
alpha	float	0.15	新值权重 (0-1)

返回 float — 滤波后的值

---

使用示例

from t265 import t265_class

t265 = t265_class()
if not t265.start():
    print("T265 启动失败!")
    return

# 起飞前校准零点
t265.autoset()

# 飞行循环
while flying:
    if t265.get_tracking_confidence() < 2:
        print("T265 置信度低，位置冻结")
        continue

    pos = t265.get_position()       # [x, y, z] in meters
    vel = t265.get_velocity()       # [vx, vy, vz] in m/s
    orient = t265.get_orientation() # [roll, pitch, yaw_err] in rad

    # PID 控制使用 pos 和 vel
    # 航向保持使用 orient[2] (yaw_err)

t265.stop()

DRY_RUN 模拟模式

当 pyrealsense2 未安装时，t265_class 自动进入模拟模式：

行为	模拟模式	真实模式
硬件初始化	跳过	rs.pipeline()
位置数据	随机漂移（σ=0.01m）	T265 VIO 输出
速度数据	随机噪声（σ=0.05 m/s）	T265 VIO 输出
置信度	恒定 3	T265 实时报告
IMU 数据	重力 + 小噪声	T265 内置 IMU
帧率	~30Hz	~200Hz

模拟模式下完整状态机可以跑通，用于桌面测试和 CI/CD。

---

已知问题

T265 冷启动概率性失败（#1）

T265 有概率在冷启动时检测失败。表现为绿灯不亮、程序报错。解决方式：物理拔插 T265 USB 口，程序会自动重连。

置信度低时位置冻结

置信度 < 2 时，_data_acquisition() 会跳过位置更新但不清零。get_position() 返回的是最后一个高置信度帧的位置。这是安全设计——避免跳变污染 PID。

---

常见问题

T265 的 Yaw 和飞控的 Yaw 有什么区别？

T265 的 Yaw 是视觉里程计估算的航向角，可能有缓慢漂移。飞控凌霄 IMU 的 Yaw 是陀螺仪积分，短期精度高但长期也会漂移。项目使用 T265 的 Yaw 做航向保持，因为它的绝对参考更好。

低通滤波 alpha=0.15 会不会太慢？

0.15 确实有一定延迟（约 6-7 帧才能跟上），但 T265 原始数据有高频噪声，直接用会导致电机抖动。如果需要更快响应，可以调大 alpha，但要测试噪声影响。当前值是实飞验证的安全选择。

get_position() 返回的是相对什么的位置？

相对起飞点（autoset() 校准的点）的位置。calibration_offset 记录了校准时的绝对位置，get_position() 返回的是当前位置减去这个偏移。Z 轴是相对校准高度，不是绝对海拔。

模拟模式下的数据有规律吗？

有。位置会随机漂移但均值为零（σ=0.01m），高度固定在 1.0m，置信度恒为 3。IMU 模拟了重力分量（az≈9.8）。可以用来测试状态机逻辑但不能测控制精度。

---

← 导航策略 | 版本选择指南 →