室内无人机定位实战：Livox Mid360激光雷达与光流融合方案

1. 室内无人机定位为什么这么难搞室内飞无人机这件事玩过的都懂。室外有GPS卫星一锁位置信息直接喂给飞控定点悬停、自动航线这些功能基本是白送。但一进厂房、仓库、地下车库或者自家客厅GPS信号要么弱到只有三四颗星要么直接归零飞控拿不到可靠的位置更新姿态稍微一偏就飘飘着飘着就撞墙了。这不是飞控不行是它压根不知道自己在哪。我最早接触室内定位方案是从光流传感器开始的。那会儿用的是PMW3901这类消费级光流模块配合超声波定高在光线充足、地面纹理丰富的环境下确实能稳住成本也低。但问题很快就暴露了地面是纯色瓷砖或者水泥地光流直接失效飞高一点超声波测距噪声大得离谱侧向风一吹或者快速平移光流跟不上位置估计直接发散。后来也试过UWB、视觉标签方案精度是够了但布设成本高换个场地就得重新部署灵活性太差。直到用上Livox Mid360这款激光雷达才算找到了一个平衡点。Mid360是Livox出的固态式激光雷达非重复扫描方式视场角大点云密度在近距离非常可观而且价格相比传统机械式多线雷达便宜不少。它输出的点云可以直接用来做SLAM或者点云配准给飞控提供位置和姿态估计。但光有雷达还不够室内飞行高度通常不高Z轴方向如果只靠雷达点云做匹配在走廊这种几何特征退化的场景里容易漂。所以我又加了一个光流传感器做辅助专门负责低速情况下的水平速度估计和高度保持。这套组合的核心思路是Mid360负责全局位置约束和建图光流负责局部速度观测和高度稳定两者通过扩展卡尔曼滤波融合输出给飞控做位置控制。听起来简单但实际调起来坑不少下面我把整个方案拆开讲。2. 硬件选型与系统架构设计2.1 为什么选Mid360而不是其他雷达市面上室内定位用的激光雷达大致分三类机械式多线雷达、固态雷达、单线雷达。机械式多线雷达比如Velodyne VLP-16精度高、点云规整但体积大、重量重、价格高装在小型四轴无人机上会严重影响载重和续航。单线雷达比如RPLIDAR系列便宜轻便但只能扫一个平面在三维空间里做定位约束不够尤其无人机有俯仰和横滚时单线雷达的数据基本没法用。Mid360属于固态雷达重量约265克尺寸小巧适合挂载在轴距250到450毫米的无人机上。它的水平视场角360度垂直视场角-7度到52度这个垂直角度很关键——无人机在室内飞行时天花板和地面的点云都能扫到给Z轴提供了足够的几何约束。非重复扫描模式下随着积分时间增加点云覆盖率会越来越高静止或低速时建图效果很好。注意Mid360的最远探测距离标称40米但室内实际有效距离受环境反射率影响很大。白色墙面反射率高能到20米以上深色或者吸光材质可能只有5到8米。做定位时不要指望它在全黑或者全吸光环境里工作。2.2 光流传感器的角色定位光流传感器我选的是PMW3901配合VL53L1X激光测距的方案。PMW3901负责水平方向的位移测量VL53L1X负责高度测量。为什么不用超声波因为超声波在室内多径反射严重尤其是地面和天花板之间来回弹测出来的高度跳变能到十几厘米对定高来说太粗糙了。VL53L1X是ToF原理精度能到毫米级响应也快适合室内低空定高。光流在这里的作用不是替代雷达而是补盲。雷达点云在走廊、长直通道这种几何特征退化的场景里前后方向的特征很少SLAM容易在前进方向上漂移。光流直接测量地面纹理的移动对水平速度的观测非常直接能有效抑制这种漂移。另外光流的数据更新频率高通常能到100Hz以上比雷达点云的10Hz快得多在飞控做高频控制时能提供更平滑的速度估计。2.3 整体系统架构整个系统的数据流是这样的Mid360通过以太网输出点云数据到机载计算机机载计算机跑SLAM算法我用的是FAST-LIO2输出雷达里程计和点云地图。光流传感器通过SPI或I2C接到飞控输出水平速度和高度。飞控内部跑EKF2或EKF3把雷达里程计、光流速度、IMU数据融合输出位置、速度、姿态估计最后给位置控制器。机载计算机我用的是一块Intel NUC或者树莓派4B看载重决定。NUC性能强能跑更复杂的SLAM和避障但重树莓派轻跑FAST-LIO2勉强够用但点云频率要降。飞控是Pixhawk 6C跑PX4固件EKF3支持外部视觉/雷达里程计输入。组件型号作用接口激光雷达Livox Mid360三维点云、SLAM定位以太网光流模块PMW3901VL53L1X水平速度、高度SPI/I2C机载计算机树莓派4B/Intel NUC跑SLAM算法UART/USB飞控Pixhawk 6C数据融合、控制-电源4S LiPo供电-3. 雷达与光流的安装与标定实操3.1 Mid360的安装位置与角度Mid360的安装位置直接影响点云质量和SLAM效果。我的经验是雷达尽量安装在无人机重心正上方或正下方避免偏心导致点云随姿态变化产生额外运动畸变。如果装在机头前方无人机俯仰时雷达会跟着上下摆动点云配准的难度会增加。安装角度上Mid360默认是水平安装垂直视场角-7度到52度。这个角度意味着它主要往上和往前看地面点云相对少。如果你希望更多地看到地面特征可以把雷达稍微向下倾斜10到15度但注意不要倾斜太多否则天花板点云会丢失Z轴约束会变弱。我试过向下倾斜15度在走廊里效果不错但在开阔厂房里天花板约束不够高度会慢慢漂。实操心得雷达安装支架一定要刚性足够。我用3D打印的PLA支架飞行时振动导致点云抖动SLAM经常丢。后来换成碳纤维板加橡胶减震柱点云稳定多了。减震柱不要选太软的否则雷达会晃也不要太硬否则振动直接传过去。中等硬度的硅胶柱比较合适。3.2 光流模块的安装与校准光流模块的安装要求比雷达更苛刻。PMW3901必须垂直向下安装镜头平面与地面平行倾斜角最好控制在2度以内。倾斜会导致光流测量出现系统性偏差飞的时候会感觉无人机总是往一个方向飘。我一开始用热熔胶固定飞了几次发现胶受热变软光流歪了后来改用螺丝加尼龙柱固定。VL53L1X的安装位置要避开螺旋桨下洗气流的影响。如果装在桨叶正下方下洗气流会导致测距噪声增大高度估计跳变。我通常把它装在机腹靠后的位置远离桨盘中心。另外VL53L1X的视场角比较窄地面如果有坑洼或者倾斜测距值会突变所以最好在飞控里加一个低通滤波截止频率设5到10Hz。光流校准分两步第一步是尺度校准让无人机在已知距离上平移比较光流积分位移和实际位移算出尺度因子。第二步是旋转校准把无人机放在原地旋转检查光流输出的方向是否和实际旋转方向一致。这两步在PX4里都有对应的校准流程跟着做就行。3.3 雷达和光流的时间同步时间同步是个容易被忽略但很致命的问题。雷达点云的时间戳和光流数据的时间戳如果不一致EKF融合时会出现观测错位表现为位置估计滞后或者震荡。Mid360输出的点云自带时间戳精度到微秒级。光流模块通过SPI读取时飞控会打上本地时间戳但SPI读取有延迟通常几毫秒。我的做法是在飞控参数里设置一个固定的光流延迟补偿值通过实验标定。具体方法是让无人机在光流有效的高度上做快速往复平移观察EKF输出的速度和光流原始速度的相位差调整延迟参数直到两者对齐。这个过程比较繁琐但调好之后位置估计的平滑度会明显提升。4. SLAM与EKF融合的参数调优4.1 FAST-LIO2的配置要点FAST-LIO2是我目前用过在室内场景下最稳的雷达SLAM方案之一。它直接对点云做迭代卡尔曼滤波不需要特征提取计算量相对可控在树莓派4B上跑10Hz没问题。配置文件里几个关键参数需要根据Mid360调整lidar_type设为Livoxscan_line设为4Mid360是4线实际上Mid360是非重复扫描这里按Livox协议配置。blind_spot设0.5米过滤掉雷达近处的无效点。filter_size_surf设0.2到0.3米这是降采样的体素大小。设太小计算量大设太大特征丢失。max_iteration设3到5迭代次数太多实时性跟不上。注意FAST-LIO2输出的里程计是雷达坐标系下的需要外参标定转换到飞控的机体坐标系。外参包括旋转和平移旋转可以用CAD模型量平移最好实际测量。如果外参不准EKF融合时会出现系统性偏差飞的时候会感觉无人机总是往一个方向偏。4.2 EKF3的外部观测融合PX4的EKF3支持多种外部观测输入包括视觉位置、视觉速度、光流、距离传感器等。雷达里程计通常以vehicle_visual_odometry话题发布包含位置、姿态、速度、角速度等信息。EKF3会把雷达里程计和IMU做融合同时用光流做速度观测用VL53L1X做高度观测。参数调优上EKF2_EV_NOISE_MD设为0表示使用外部指定的噪声参数。EKF2_EV_P_NOISE是位置观测噪声雷达SLAM的位置精度在室内通常能到厘米级我设0.05到0.1。EKF2_EV_V_NOISE是速度观测噪声设0.1到0.2。光流的噪声参数EKF2_OF_N_MIN和EKF2_OF_N_MAX根据实际测试调整通常设0.1和0.3。这里有个坑雷达SLAM在初始化阶段输出不稳定位置可能跳变。如果EKF3在雷达还没收敛时就融合它的数据会导致滤波器发散。我的做法是在飞控里加一个判断等雷达里程计连续输出稳定位置超过2秒后再启动EKF融合。PX4里可以通过EKF2_EV_DELAY参数设置延迟或者在地面站里手动控制融合开关。4.3 光流与雷达的权重分配光流和雷达在EKF里的权重不是固定的需要根据飞行状态动态调整。低速悬停时光流的速度观测很准可以给高权重快速平移时光流可能因为地面纹理模糊而失效这时候应该降低光流权重更多依赖雷达。PX4的EKF3没有直接的动态权重接口但可以通过设置光流的最大速度限制来实现类似效果。EKF2_OF_MAX_VEL设2到3米每秒超过这个速度光流观测自动降权。另外光流的高度观测和VL53L1X是绑定的。如果VL53L1X测距超过有效范围比如飞太高高度观测会自动失效EKF会切换到气压计或者雷达Z轴。室内飞行高度通常不超过3米VL53L1X的量程够用但要注意地面反射率。深色地面反射弱测距可能跳变这时候可以在飞控里加一个中值滤波。5. 飞行测试与常见问题排查5.1 首次飞行测试流程第一次试飞不要直接切定点模式先用手动模式或者自稳模式飞观察无人机的基本姿态是否正常。然后切高度模式检查VL53L1X定高是否稳定。最后再切定点模式看位置保持效果。我通常的测试步骤是地面通电检查雷达点云是否正常输出光流数据是否有跳变。手动起飞悬停1米高度观察无人机是否往一个方向漂。切高度模式松杆看高度是否稳定在±10厘米以内。切定点模式轻推杆让无人机平移松杆看是否能停住。做一次快速往复平移观察位置估计是否滞后或震荡。实操心得第一次切定点模式时最好在无人机下方铺一块纹理丰富的地毯或者放几个纸箱。光流在纯色地面上容易失效有纹理的地面能让光流工作更稳定。如果地面实在没法改那就把光流权重调低更多依赖雷达。5.2 常见问题速查表现象可能原因排查方法解决方案定点模式无人机画圈雷达外参旋转不准检查雷达坐标系到机体的旋转矩阵重新标定外参高度缓慢漂移雷达Z轴约束弱观察点云中天花板和地面点数量调整雷达俯仰角或增加光流高度权重快速平移后位置跳变光流失效或雷达丢帧查看光流质量值和雷达里程计频率降低光流权重检查雷达振动起飞后往一个方向偏光流安装倾斜或外参平移不准地面旋转测试光流方向重新校准光流和雷达外参EKF频繁切换观测源噪声参数设置不当查看EKF日志中的创新序列调整观测噪声参数雷达SLAM丢失环境特征退化或快速旋转检查点云匹配残差降低飞行速度增加环境纹理5.3 避坑指南我踩过的那些坑第一个坑是雷达供电不足。Mid360的峰值电流能到2A以上如果直接从飞控的5V输出取电电压会被拉低雷达会间歇性重启。我一开始没注意飞的时候雷达点云突然断流SLAM直接丢失无人机飘了。后来单独给雷达配了一个5V 3A的BEC问题解决。第二个坑是光流镜头脏了。室内飞行灰尘大光流镜头几天不擦就一层灰光流质量值直接掉到20以下。现在我每次飞之前都用镜头布擦一下养成习惯。第三个坑是EKF初始化顺序。PX4的EKF3在启动时需要所有观测源都有效才会进入导航模式。如果雷达SLAM还没收敛光流也没校准好EKF会一直卡在初始化状态。我的做法是先在地面静止等待雷达SLAM收敛通常10到20秒再解锁起飞。第四个坑是飞行速度太快。室内空间有限快速飞行时雷达点云运动畸变严重SLAM跟不上。我建议室内定点飞行速度控制在1米每秒以内最快不要超过2米每秒。超过这个速度再好的算法也救不回来。6. 方案扩展与进阶玩法这套Mid360加光流的方案基础功能是室内定点悬停和低速航线飞行。如果想进一步扩展有几个方向可以尝试。一是加入视觉避障。Mid360的点云本身就可以用来做避障通过局部代价地图或者VFH算法可以在飞控里实现简单的避障功能。PX4支持obstacle_distance话题输入把雷达点云投影成二维距离图就能用。二是多机协同。室内多机飞行时每台无人机的雷达点云可以共享通过一个中心节点做全局地图融合提高定位精度和鲁棒性。这个需要额外的通信链路和地图融合算法复杂度较高但效果很好。三是自主探索与建图。利用雷达SLAM输出的点云地图结合前沿点检测算法可以让无人机自主探索未知室内环境并建图。这个在仓库盘点、厂房巡检场景下很有用。四是更换更轻的机载计算机。树莓派4B跑FAST-LIO2已经比较吃力了如果换成Jetson Orin Nano或者RK3588可以跑更复杂的SLAM和避障算法同时重量增加不多。最后分享一个小技巧雷达和光流的融合参数不是调一次就一劳永逸的。换一个场地地面材质、天花板高度、环境纹理都会变参数可能需要微调。我通常会在新场地先做一次低速悬停测试观察EKF日志里的位置和速度估计如果噪声大就适当调高观测噪声参数如果响应慢就调低。这个过程花不了几分钟但能避免很多炸机风险。