T265+PX4视觉定位保姆级教程：从驱动安装到EKF2融合与MAVROS桥接

我第一次把 T265 接到 Pixhawk 上时无人机在地面站里显示的位置跟实际位置永远差着 90 度差点把满屋子设备撞翻。后来排查下来才发现问题不在硬件而在整个数据链路里有一层没人明说的坐标系转换。这篇文章想把这套链路完完整整拆开讲一遍从 Ubuntu 20.04 环境怎么准备、T265 驱动怎么装到 PX4 固件怎么编、EKF2 参数怎么配再到视觉数据怎么真正喂进飞控、联调时怎么验证。每一步我都会给出可以直接抄作业的命令和参数并把那些最容易让人原地爆炸的坑提前指出来。内容更适合刚接触 PX4 视觉定位、想在室内或者弱 GPS 环境下做自主飞行的同学也欢迎已经在折腾、但卡在某个环节的老哥们对照排查。标题里的“保姆级”不是客套话。我踩过的那些坑很多并不是操作手册里写了但你没看到而是手册默认你已经懂了一些东西。所以这篇文章里我会像旁边坐着一个朋友那样把每一步背后的“为什么”也讲清楚而不只是告诉你“按这个跑就会通”。1. 先搞清这套系统的数据链路T265、PX4 和 EKF2 分别扮演什么角色很多新手一开始就把所有精力扑在安装上结果驱动装好了、固件编译过了飞机却还是乱飘。根本原因是没有先建立一张系统的认知地图。这一章节不讲命令先讲清楚这套系统里三个关键角色的分工以及数据是怎么流起来的。1.1 T265 到底是什么VIO 传感器不是“SLAM 相机”Intel RealSense T265 是一颗视觉惯性里程计传感器内置了两个广角灰度相机加一颗 IMU核心工作是实时输出“相机本体相对启动原点的位姿估计”。严格说它是 VIOVisual-Inertial Odometry不是 SLAM。它不做建图、不生成点云也不会告诉你“前方有没有障碍”。它的输出只有两个核心信息位置和姿态频率大概在 30Hz 左右。这个区别特别重要。有些人拿到 T265 以后以为接上它就能避障这是完全两码事。T265 的价值在于在没有 GPS 的环境里它能以低延迟给出相对运动估计替代或者辅助惯导完成定位。把它类比成“睁着眼睛走路的惯性导航”更贴切。T265 还有一个特点它输出的位姿是相对自己上电那一刻的原点。也就是说它没有绝对坐标只有相对坐标。这意味着无人机每次上电后的“世界原点”都可能不一样所以起飞前必须等它完成初始化并且在整个飞行过程中不能出现重新定位否则飞控会认为位置发生了突变。1.2 PX4 的视觉融合链路EKF2 拿到数据以后做了什么PX4 内部从 1.11 版本起统一使用 EKF2 做多传感器融合状态估计。EKF2 会把 IMU 积分出来的捷联推算结果和 GPS、气压计、磁力计、光流、视觉里程计等外部观测做融合输出稳定平滑的位置、速度和姿态估计。当视觉里程计接入后EKF2 里有两种融合思路。一种是直接把 T265 的位置估计当“视觉位置观测”融合这对应着 EKF2_AID_MASK 参数里的 vision position 位另一种是把 T265 经过差分得到的线速度当“视觉速度观测”融合对应 vision velocity 位。两套都能用实践里最常见的做法是同时融合视觉位置和视觉偏航让视觉在水平定位和航向两个维度上参与校正。1.3 完整数据流从 T265 到 EKF2 中间经过几条桥整条链路可以画成一条直线T265 通过 USB 3.0 接到机载电脑或树莓派→ librealsense 驱动读取原始数据 → realsense-ros 发布 ROS 话题里的里程计数据 → 桥接节点/MAVROS 把姿态和位置转成飞控认识的 MAVLink ODOMETRY 消息 → Pixhawk 通过串口/MAVLink 收到消息 → EKF2 消费视觉观测并参与状态估计。你看到的“无人机悬停稳定”实际上是这条链路上每一环都正常工作后的综合结果。任何一个环节掉链子轻则定位信息时断时续重则飞控直接拒绝使用视觉数据退回纯惯导模式。这也是为什么后面每一章节我都会强调“怎么验证这一环真的通了”。2. Ubuntu 20.04 装机与 ROS Noetic 部署先滚平两块硌脚石头T265 的官方驱动和 realsense-ros 在 Ubuntu 20.04 ROS Noetic 这套组合下最省心我也是在把系统大量重装之后才稳定在这套环境上的。如果你已经装好了系统和 ROS这一章可以快速跳过如果你是第一次从 Windows 过来请耐着性子看完这两个细节。2.1 双系统安装后的时钟与启动引导问题标题下的热搜里有一条是“Ubuntu 20.04 双系统法安装”。我自己的做法也是双系统但装完几乎都会遇到两个问题一是每次进 Windows 再回 Ubuntu系统时间会快 8 个小时二是启动引导界面偶尔不见了。时间问题的根源很简单Windows 默认把硬件时钟当作本地时间Ubuntu 默认把硬件时钟当作 UTC 时间。解决方法是让 Ubuntu 也用本地时间终端执行timedatectl set-local-rtc 1 --adjust-system-clock执行完重启就没问题了。引导问题大多发生在主板同时挂着 Windows Boot Manager 和 Ubuntu 的 GRUB 时。我建议在 BIOS 里把 Ubuntu 所在硬盘设为第一启动项这样默认进 GRUB然后改/etc/default/grub里的默认启动项并执行sudo update-grub。这一步不做后面启动界面经常要手忙脚乱按 F12很烦。2.2 ROS Noetic 的几个依赖坑Ubuntu 20.04 对应 ROS Noetic安装命令网上遍地都是我在这里只提醒几个我实际踩到过的点。第一装完 ROS 后一定记得执行sudo rosdep init rosdep update这一步不做的后果是后面编译功能包时 rosdep 检依赖会卡到天荒地老。如果你遇到 rosdep 网络问题多试几次基本能过去。第二MAVROS 这边除了主包还要装mavros-extras更重要的是要执行一次地理数据集的下载脚本否则 MAVROS 启动时会在加载地球模型处报错sudo apt install ros-noetic-mavros ros-noetic-mavros-extras sudo /opt/ros/noetic/lib/mavros/install_geographiclib_datasets.sh很多人做到sudo apt install ros-noetic-mavros就停了结果后面一直卡在 MAVROS 连不上飞控其实日志里写的是 geography 数据缺失容易让人误判。第三给 Ubuntu 20.04 装网络配置的时候别去系统设置里反复点图形界面。直接在/etc/netplan/01-network-manager-all.yaml里配置固定 IP 更适合机载场景network: version: 2 renderer: NetworkManager ethernets: eth0: dhcp4: no addresses: [192.168.1.100/24] gateway4: 192.168.1.1 nameservers: addresses: [8.8.8.8, 114.114.114.114]这个配置在通过局域网 SSH 连机载电脑时非常实用不用每次开机都去找动态 IP。3. T265 驱动安装决定成败的 USB 带宽与固件版本问题T265 的驱动安装本身不复杂复杂的是它对外部环境相当挑剔。这一部分我会把环境、编译、验证、避坑码成一个完整流程其中 USB 带宽那一节请务必仔细看这是 T265 最常见的“假死”来源。3.1 librealsense 源码编译与版本选择为什么我推荐离开 apt 装最新版Ubuntu 20.04 的 apt 源里虽然有 librealsense但版本通常有点旧T265 的固件兼容性也容易出问题。我更推荐直接源码编译。依赖安装如下sudo apt update sudo apt install git cmake build-essential libssl-dev libusb-1.0-0-dev libgtk-3-dev libglfw3-dev pkg-config克隆源码时建议直接切到经过验证较稳的 tag比如 2.53.1。我试过 2.50.0、2.53.1 和 2.54 系列T265 在 2.50.0 和 2.53.1 上都很稳2.54 之后的版本在部分机器上出现过固件版本不匹配的提示网上也有不少反馈所以图省心就用 2.53.1。git clone https://github.com/IntelRealSense/librealsense.git cd librealsense git checkout v2.53.1然后执行两个脚本sudo cp config/99-realsense-libusb.rules /etc/udev/rules.d/ sudo udevadm control --reload-rules sudo udevadm trigger第一句是把 udev 规则装上否则普通用户无法访问 T265会出现权限错误。接着配置编译这里有一个关键选项mkdir build cd build cmake .. -DBUILD_EXAMPLEStrue -DFORCE_RSUSB_BACKENDtrue -DCMAKE_BUILD_TYPErelease make -j$(nproc) sudo make installFORCE_RSUSB_BACKENDtrue这个选项的意思是强制使用用户空间的 USB 后端不依赖内核的 uvcvideo 模块。实测下来T265 和 D435i 混插时经常出现设备掉线开启这个选项后稳定很多。代价是 CPU 占用会高一丢丢对机载电脑来说完全可以接受。装完以后插上 T265 执行realsense-viewer能看到设备列表并显示“T265”字样方向传感器、IMU 数据都在动才说明驱动层通了。如果这里 H.264 流或者 IMU 没有任何输出绝对不要继续往下走先排查硬件和 USB。3.2 realsense-ros 编译catkin 不能少的东西驱动层通了以后下一步是装 realsense-ros把 T265 的原始数据转成 ROS 话题。推荐在catkin_ws里编译。cd ~/catkin_ws/src git clone https://github.com/IntelRealSense/realsense-ros.git cd ~/catkin_ws catkin_make依赖上面提到ddynamic_reconfigure和vision_msgs如果编译报错缺包就先sudo apt install ros-noetic-ddynamic-reconfigure很多人在这一步卡住是因为用了 Python 3 的pip装了某些依赖但 ROS Noetic 的包是用 apt 管理的。记住所有 ROS 相关依赖优先用 apt 装尽量少用 pip。编译完成后启动 T265 节点roslaunch realsense2_camera rs_camera.launch然后在另一个终端rostopic hz /camera/odom/sample正常情况下会看到 30Hz 左右的里程计频率。只有/camera/odom/sample这个 nav_msgs/Odometry 类型的消息实实在在在刷T265 这条路才算打通。3.3 USB 带宽与供电问题的完整排查链路T265 要求 USB 3.0 带宽但这不等于插在蓝色口上就一定满足。我用lsusb -t排查了很多次命令输出里能看到设备实际协商的速度lsusb -t如果你看到 T265 那条设备树上写着 5000M说明运行在 USB3.0如果显示 480M说明它实际跑在 USB2.0 模式下这时候不要指望稳定输出。常见原因是线材是 USB2.0 的或者机载电脑的 USB3 口虚标。供电则是另一个隐蔽问题。我在树莓派 4B 上试过直接插板载 USB3 口时T265 偶尔会“消失”几秒然后 realsense-viewer 里死活找不到设备。最后是换了一个带独立供电的 USB3 HUB 解决的。排查链路我建议按这个顺序走先看lsusb -t是不是 5000M再看/var/log/syslog里有没有 USB device descriptor 报错最后才去怀疑硬件坏。T265 本身相对皮实绝大多数故障都出在供电和线材上。3.4 T265 坐标系与话题输出绕不开的 REP 103 约定T265 的 SDK 原始坐标系是 x 向右、y 向下、z 向前但 realsense-ros 中间层会自动转成 ROS 的 REP 103 标准也就是 x 向前、y 向左、z 向上即 ENU 惯性系。这意味着你从/camera/odom/sample里拿到的位置和姿态已经可以直接被 ROS 生态里的 MAVROS 消费不需要自己做坐标系变换。这也是我后面推荐 MAVROS 方案的核心原因之一。如果你自己写程序直接发 MAVLink这部分坐标转换就得自己处理非常容易翻车后面第 5 章会专门讲。4. PX4 固件编译与 EKF2 视觉融合参数把视觉真正喂进飞控驱动层搞定以后飞控这边也要做好准备。不是拿一根线把 Pixhawk 和电脑连上就能直接用视觉数据固件版本、编译工具链和 EKF2 参数三者都要对上。4.1 固件版本选择与源码编译Ubuntu 20.04 上我推荐 PX4 v1.14.3。这个版本对 ROS Noetic 的兼容性最好EKF2 的视觉融合也很成熟。v1.15 也可以跑但新版本对编译工具链要求高一些容易引入和 cmake、gcc 相关的环境问题新手没必要一上来就追新。克隆源码时要带子模块cd ~ git clone --recursive https://github.com/PX4/PX4-Autopilot.git cd PX4-Autopilot git checkout v1.14.3 git submodule update --init --recursive编译 Pixhawk 固件make px4_fmu-v5_default这一步会拉取大量工具链耗时比较长耐心等。如果你手头是 Pixhawk 6X 这类飞控目标名换成px4_fmu-v6x_default即可命令格式是一样的。如果编译过程中报gcc: internal compiler error或者某些 c 标准头文件找不到先检查是不是系统里有多个 gcc 版本混用。Ubuntu 20.04 自带的 gcc-9 编译 v1.14.3 毫无压力不需要折腾升级。4.2 EKF2 参数逐项说明为什么这么设这是整个教程里最核心的参数部分。烧好固件以后在 QGroundControl 的参数界面里搜并修改以下参数。参数名推荐值说明EKF2_AID_MASK12二进制 1100即 bit2(视觉位置)bit3(视觉偏航)含义是让 EKF2 融合视觉位置和视觉偏航EKF2_EV_NOISE_MD0.6视觉位置噪声标准差单位米。太小会让视觉抖动直接传导到位置环太大会让视觉基本不参与修正EKF2_EV_VEL_NOISE_MD0.3视觉速度噪声只在用视觉速度融合时需要但建议一并调整好EKF2_EV_POS_X0.0T265 相对机体重心的前向偏移正方向为机头前EKF2_EV_POS_Y0.0右向偏移正方向为机头右EKF2_EV_POS_Z0.0向下偏移正方向为机头下。如果 T265 装在重心上方 3cm就填 -0.03EKF2_REQ_VISION_ACC0.5视觉位置融合触发的最低精度要求EKF2_REQ_VISION_VEL0.5视觉速度融合触发阈值关于 EKF2_AID_MASK网上能看到很多教程推荐设 24其实那是 bit3(视觉偏航)bit4(视觉速度)的组合含义是不把视觉位置作为外环观测而是用视觉速度辅助惯导。这个方案在相机颠簸大、位置估计噪声大的场景下也很常用。两种都能跑我推荐先按 12 设因为简洁直观出问题也好排查。这里有个特别容易迷惑人的点如果你在旧教程里看到SYS_MC_EST_GROUP 2那是在 PX4 1.10 及以前版本里切换到有视觉支持的估计器用的v1.14 里这个参数已经废弃千万别再设置它否则反而可能让参数系统报错。版本差异带来的参数混乱问题比硬件问题更容易把人绕晕。4.3 安装偏移参数 EKF2_EV_POS_* 为什么不能随便填EKF2 在做传感器外参融合时需要知道 T265 安装在机体坐标系中的哪个位置。T265 离机体中心越远这个偏移对融合结果的影响就越大因为视觉观测的位置是相机坐标系原点的位置不是飞机重心的位置。我见过一个案例T265 装在机头前方 20cm 处EKF2_EV_POS_X没填结果飞机 hover 时始终前后震荡。EKF 一直认为自己位置在机头那一点和重心位置差了 20cm导致位置环不断修正悬停品质非常差。把EKF2_EV_POS_X填成 0.2 后立刻稳定下来。这个参数属于“不会立刻爆雷、但一定影响飞行品质”的细节装机时用尺子量一量最稳妥。5. 两条把 T265 数据送入飞控的通路MAVROS 转发与 MAVLink 直发飞控只认 MAVLink不认识 ROS所以必须有一个“翻译官”把 T265 的里程计转成 MAVLink ODOMETRY 消息。这一章我给出两条路一条适合大多数人一条适合想绕开 ROS 体系、直接写轻量程序的老手。5.1 推荐路线写一个 Python 桥接节点转发到 MAVROSMAVROS 是 ROS 和 PX4 之间的标准桥它已经处理好了 ENU 到 NED 的坐标转换、四元数换算、时间戳对齐这些脏活我们只要把/camera/odom/sample转发到/mavros/vision_pose/pose即可。建立工作区以后创建一个脚本t265_to_mavros.py#!/usr/bin/env python3 import rospy from nav_msgs.msg import Odometry from geometry_msgs.msg import PoseStamped pub rospy.Publisher(/mavros/vision_pose/pose, PoseStamped, queue_size1) def odom_cb(msg): pose PoseStamped() pose.header msg.header pose.header.frame_id map pose.pose msg.pose.pose pub.publish(pose) def main(): rospy.init_node(t265_to_mavros) rospy.Subscriber(/camera/odom/sample, Odometry, odom_cb) rospy.spin() if __name__ __main__: main()为什么不直接用/camera/pose/sample这个 PoseStamped 话题 remap 到/mavros/vision_pose/pose也可以但/camera/pose/sample是前端 SLAM 位姿话题而/camera/odom/sample是里程计话题两者在 T265 内部计算方式略有差异odom 话题更适合做融合。用 Python 节点转发最大的好处是以后可以顺手在这个节点里做滤波、频率限制、坐标偏移补偿不用改 MAVROS 配置。启动顺序建议是先起飞控的 MAVROS再启动 T265 节点最后运行桥接脚本。MAVROS 启动命令roslaunch mavros px4.launch fcu_url:/dev/ttyUSB0:921600如果固件和 MAVROS 都正常MAVROS 日志里会出现FCU: detected字样。5.2 MAVROS 方案的验证方法不能只看“绿灯亮”验证桥接是否真的把数据送进了飞控分两步。第一步用命令看 ROS 侧rostopic echo /mavros/vision_pose/pose有数据在刷说明桥接没问题。第二步更重要在 QGroundControl 里看飞控到底有没有启用视觉融合。打开“日志分析”或者看 EKF2 状态消息确认vision_position和vision_yaw对应的融合标志位置 1。只看 ROS 侧数据在刷是完全不够的EKF2 可能因为触发条件不满足而拒绝使用视觉观测。5.3 进阶路线用 pymavlink 直接发 ODOMETRY 消息如果机载电脑资源紧张或者你想摆脱对 ROS 的依赖可以直接用 pymavlink 把 ODOMETRY 消息发给飞控。核心代码骨架如下from pymavlink import mavutil from pymavlink.quaternion import Quaternion master mavutil.mavlink_connection(/dev/ttyUSB0, baud921600) master.wait_heartbeat() # 从 T265 SDK 直接取位姿 pos_x, pos_y, pos_z ... quat_w, quat_x, quat_y, quat_z ... master.mav.odometry_send( time_usec0, frame_idmavutil.mavlink.MAV_FRAME_LOCAL_NED, child_frame_idmavutil.mavlink.MAV_FRAME_BODY_FRD, xpos_x, ypos_y, zpos_z, q[quat_w, quat_x, quat_y, quat_z], vx0.0, vy0.0, vz0.0, roll_speed0.0, pitch_speed0.0, yaw_speed0.0, pose_covariance[0] * 21, velocity_covariance[0] * 21, estimator_type3, # MAV_ESTIMATOR_TYPE_VIO reset_counter0 )这里最大的坑在坐标系。T265 在 realsense-ros 里输出的 ENUx 前、y 左、z 上数据要转成 PX4 期望的 NED 风格。简单地从数值上讲需要把 y、z 取反但四元数也得对应变换而且还需要处理 T265 启动时航向可能与机头并不对齐的问题。这一层不处理干净飞控拿到的是错误的相对运动后果比不接视觉还严重。所以我的建议很直白第一次上机做视觉定位不要选这条路。MAVROS 虽然多重一层 ROS但它把坐标变换这类容易出错的部分处理好了。等到你对系统理解足够深再考虑直接写 MAVLink 也不迟。两种方案对比如下方案复杂度坐标处理调试友好度资源占用MAVROS 转发低自动处理高rostopic 直接看数据较高pymavlink 直发高手动处理低出错要靠日志分析低6. 联调验证与翻车现象排查起飞前只看这套清单到了这一步驱动、固件、桥接、参数都配完了接下来就是联调验证。这一章节我按“逐级验证”的思路来写每一级都给出能直接判断成败的方法再给出一份我这些年遇到过的异常现象对照表。6.1 逐级验证从 T265 到 EKF2 一层层确认第一级T265 自身执行rostopic hz /camera/odom/sample确认 30Hz 左右。如果频率低于 10Hz先查 USB3.0 模式再查供电。第二级桥接执行rostopic hz /mavros/vision_pose/pose确认数据频率保持。这里如果没数据回看 MAVROS 是否检测到飞控、桥接脚本是否运行。第三级EKF2 融合状态在 QGroundControl 的“车辆状态”或者日志里查看 estimator status重点看视觉位置和视觉航向对应的融合标志是否激活。如果没激活多半是 EKF2_AID_MASK 没设对或者视觉噪声参数导致 EKF2 一直不信任这个观测源。第四级物理验证。拆掉螺旋桨把无人机放在手里轻轻前后左右平移观察地面站的飞机模型是否跟随移动。如果飞机模型的位置和姿态与实际移动方向吻合说明整套链路闭环了。这一步不要去外面飞就在桌上做安全第一。6.2 常见异常现象对照表这些现象和解决办法基本是这几年折腾视觉定位经验的浓缩版直接对照着查就可以。现象可能原因处理方法T265 设备间歇性消失USB2.0 线材或供电不足换 USB3.0 线材使用带独立供电的 HUBlsusb -t检查速率T265 在 realsense-viewer 里没有画面固件版本和驱动不兼容退回 librealsense v2.53.1 重编/camera/odom/sample频率只有几Hz系统负载过高或 USB 带宽被挤占关掉 realsense-viewer 可视化检查后台进程必要时插独立 USB 控制器MAVROS 连接飞控失败波特率不匹配或 geographiclib 数据未安装确认串口设备名执行install_geographiclib_datasets.sh桥接着了但 QGC 显示位置不动EKF2_AID_MASK 未包含视觉位置位检查参数bit2 是否置 1手握飞机移动地面站位置方向相反T265 安装方向与机头不一致调整安装方向或在校准参数里做补偿悬停时前后震荡EKF2_EV_POS_X/Y/Z 安装偏移没填用尺子量 T265 到重心的偏移填进参数起飞后飞机快速侧飞视觉偏航未融合或方向反了确认 EKF2_AID_MASK 包含 bit3检查 T265 安装方向飞行中视觉定位偶尔丢失飞机猛飘一下光照或纹理不足T265 lost tracking保证环境灯光充足不要对着白墙飞给地面加纹理标识6.3 室内自主飞行前的检查清单起飞前花五分钟过一遍这份清单能避免绝大多数“炸机式”问题。第一T265 上电后先静止 5 秒再解锁起飞。它启动瞬间会记录一个初始状态如果启动时还在动后续位姿可能出现持续偏移。第二确认 EKF2 里视觉融合标志已经激活而不是只在 ROS 侧看到数据。第三检查安装偏移参数保证 T265 相对机体重心的三个方向偏移数值都填了哪怕你认为“几乎在重心上”也填个 0.01 之类的估算值避免参数系统不确定。第四环境检查室内灯光要够亮地面最好有纹理。T265 的工作距离和纹理敏感度是硬限制光线太暗或者大面积白墙都会让它瞬间失去跟踪。第五先拆桨测试用手持方式模拟平移、偏航确认 QGC 里飞机模型运动和实际一致再装桨进行低高度悬停测试。最后再分享一个习惯做了这么多次视觉定位联调我最大的体会是永远别同时改多个变量。很多朋友一上来就同时调 EKF2_AID_MASK、改噪声参数、然后又怀疑 T265 装歪了结果一个变量调对了但被另一个变量掩盖整个下午都在原地打转。每次只改一个参数保存后复测一次配合 QGC 的日志对比效率反而最高。这套流程看起来很繁琐但一旦打通后续再做基于视觉的自主飞行、目标跟踪都会顺很多。毕竟定位稳了所有上层功能才有得玩。