TSLAM室内定位系统详述

室内自主探索无人机系统

一，概述

室内无人机飞行系统主要针对室内无卫星导航信号，无合作目标，可对室内空间进行侦察、重复探测、飞行过程中能够躲避障碍、能自主任务规划，实现对室内各个空间的侦察或巡检等作业。为了满足此等作业要求，我们设计出地下空间环境自主飞行无人机系统总体方案。主要内容包括：

1)完成室内无源自主定位系统（TSLAM定位系统）

2)基于Linux系统超强运算能力的飞控硬件

3)适合室内自主起飞降落的飞控软件

4)实现自主路径探索的飞行导航软件

5)基于视觉引导降落的视觉系统

6)适合室内飞行的无人机飞行平台

二，系统框架与结构框架

系统框架

结构框架

三，内容详述

1，TSLAM定位系统

SLAM是采用同步定位与地图构建的一种无源建图定位技术，而TSLAM定位系统作为一整套室内定位系统的解决方案，其独创的tiny-EKF融合算法，已融合包含气压计、高精度imu、磁罗盘、激光雷达以及高精度光学编码里程计等传感器的数据融合。

SLAM系统近年来主在智能机器人领域得到了广泛的应用，比如扫地机器人、无人车等。目前SLAM技术中Cartographer开源框架比较先进,该框架适合通用性的SLAM，但对于无人机这种非二维平面的运动物体来讲，该框架要做到稳定的SLAM定位效果较差，容易产生发散漂移。目前TSLAM定位系统主要针对无人机重新开发算法，更符合无人机在室内环境的自主定位飞行作业。

优点如下：

1）、基于环境自然导航，无需对客户现场环境做任何的改造

2）、定位精度满足绝大部分场景应用

3）、配套相应的操作软件，易学易用

4）、变更使用场景简单，客户自由操作

5）、环境亮度要求低，日常光亮环境、昏暗环境皆可作业

6）、无惧磁干扰、多金属环境

2，TLSAM定位系统优势

1），对比相关开源项目

Cartographer

Hectorslam

Gmapping

Gmapping是基于粒子滤波的SLAM算法，而Cartographer和Hectorslam 都是基于非线性优化的，现在已经很少用滤波方法来做SLAM了，绝大多数还是使用最小二乘法。

Cartographer是有后端的SLAM算法，而Hectorslam是无后端的，前者更适用于以建图为重点的应用，后者更适合以定位为重点的应用。

Cartographer在无人驾驶平台上会遇到如下问题：

（1）定位噪声大：在载具不动的情况下，会产生10cm左右漂移。

（2）定位错误：由于载具运动会在快速移动或者大角度倾斜移动的情景下会产生匹配错误，在融合了imu的情况下坐标偏差达到10m以上。

（3）效率太低：比如在机载飞控板卡上跑cartographer时常达到400%的cpu 负载（一个核是100%）

（4）通讯受阻：

目前使用ROS通信方式为Cartographer为通信框架，ROS属于实验性验证性工具框架，在ROS topic话题中使用TCP本地传输数据，数据从用户层进入内核，然后再回到用户层，造成巨大延迟。不符合实时要求，会造成通讯延迟，稳定性较差。该项目优化建立新的通讯方式，直接使用指针传递函数。数据通信几乎实现零延时效果。

2），对比算法进行优化达到效果

（1）优化相关算法实现准确的同步定位，累计误差减小。

（2）针对载具姿态变化问题做出算法研究，加入融合ekf滤波以后的高精度imu数据做相关数据处理，让载具达到至少在30度倾斜角时，仍然良好建图定位效果。

（3）专门对激光匹配做出优化算法，让载具在速度很快的情况下也能良好定位，能满足正常大机动。

（4）传统算法很多冗余的不分，需要对算法实现减法处理抛弃不需要的组件，让框架轻简，能挺高运算速率。

（5）抛弃ros通信，实现简洁通信协议让整个系统的数据链路更加快速。

（6）实现了对位姿先验的支持，可以不限于使用激光测距来实现定位，同其他SLAM算法一样，使用轮式里程计，视觉里程计，加速度计均可以作为SLAM匹配器的先验，让SLAM 更精确，可以排除大量的匹配错误，让SALM更平滑。

（7）完成对后验位姿的准确度判定算法升级，如果判定得到有明显的匹配错误，则丢弃当前帧定位与建图，明显减少了在场景特征稀少的区域出现位姿发散的问题。

3，Tslam构成组件

1），点云滤波

tslam滤波方法是结合了体素滤波和半径滤波的，首先我们要改变半径体素滤波的规则，我们不再以固定半径的立方体格子作为滤波单位，转而使用变化的半径，考虑激光雷达的特性，最好是以弧度与半径作为单位格：

类似上图的蛛网状扇形切分个数作为一个参数，一圈一圈的半径切分个数也作为一个参数。以切分出的扇区作为滤波单元，好处显而易见，当激光雷达扫的很远的时候格子体积自然而然的增大了。然后再使用半径滤波的思想，如果某个扇区内的点云个数大于设定的阈值，则保留，也不再取几何中心点代替，如果小于，我们直接剔除。这种方法在参数设置合理的情况下几乎可以完美的剔除95%以上的离群点。

2）. 激光雷达除畸变

在机器人运动过程中，每个激光点都在不同的基准位姿上产生。

激光扫描时伴随着机器人的运动，每个角度的激光数据都不是瞬时获得的，当激光雷达扫描的频率比较低的时候，机器人运动带来的激光帧的运动畸变是不能被忽略的。例如扫描频率是5Hz的激光雷达，一帧数据的收尾时间差是200ms，如果机器人以0.5m/s的速度沿着x 方向行走并扫描前面的墙体，那么200ms后尾部的测量距离和首部的测量距离在x方向上就差10cm。所以如果不是高频扫描，这种运动畸变是不容忽视的。

去除激光雷达运动畸变的原理是把一帧激光雷达数据的每个激光点对应的激光雷达坐标转换到不同时刻的机器人里程计上

由于imu adis16488的加入，我们可以把它作为一个高精度的惯性里程计使用，我们可以得到一个搞到1khz的位姿惯导，将激光雷达的每一个激光点，都精确的定位到真实的物理坐标中。

流程：

·已知当前激光帧的起始时间t1

·两个激光束之间的时间间隔t

·惯性里程计数据按照时间顺序存储在一个队列中

·求解当前帧激光数据中的每一个激光点对应的里程计数据（机器人位姿）

·根据求解的位姿把所有的激光点转换到同一坐标系下

·重新封装成一帧激光数据发布出去

经过此流程以后，得到的激光点云的畸变被控制在了以往的1%以内，并且，使用的惯性传感器越好，效果越大，实测使用adis16448去除畸变后，飞行载具3m/s的速度运动，对建图、定位，均无任何副作用。

3），点云匹配

Tslam通过对获取到的点云建立误差函数进行点云匹配

若将图形xy平面看做姿态位置维度

Z轴看做误差维度（z轴越往上误差越小），

误差函数在三维空间中具有如下属性：