TSLAM室内定位系统详述

1、室内自主探索无人机系统，概述室内无人机飞行系统主要针对室内无卫星导航信号，无合作目标，可对室内 空间进行侦察、重复探测、飞行过程中能够躲避障碍、能自主任务规划，实现对 室内各个空间的侦察或巡检等作业。为了满足此等作业要求，我们设计出地下空 间环境自主飞行无人机系统总体方案。主要内容包括：1）完成室内无源自主定位系统（TSLAM定位系统）2）基于Linux系统超强运算能力的飞控硬件3）适合室内自主起飞降落的飞控软件4）实现自主路径探索的飞行导航软件5）基于视觉引导降落的视觉系统6）适合室内飞行的无人机飞行平台二，系统框架与结构框架ext odomLidarIMUfilterTSLAMArdupi

2、lot等控制软件ext odomLidarIMUfilterTSLAMArdupilot等控制软件系统框架自主巡检无人机硬件平台高侑度光电耦合星程计LIDAR飞行控制器1平台高侑度光电耦合星程计LIDAR飞行控制器1两臂S rMU结构框架三，内容详述1, TSLAM定位系统SLAM是采用同步定位与地图构建的一种无源建图定位技术，而TSLAM定位系统作为一整 套室内定位系统的解决方案，其独创的tiny-EKF融合算法，已融合包含气压计、高精度imu、 磁罗盘、激光雷达以及高精度光学编码里程计等传感器的数据融合。SLAM系统近年来主在智能机器人领域得到了广泛的应用，比如扫地机器人、无人车等。 目前

3、SLAM技术中Cartographer开源框架比较先进,该框架适合通用性的SLAM，但对于无人 机这种非二维平面的运动物体来讲，该框架要做到稳定的SLAM定位效果较差，容易产生发 散漂移。目前TSLAM定位系统主要针对无人机重新开发算法，更符合无人机在室内环境的 自主定位飞行作业。优点如下：1）、基于环境自然导航，无需对客户现场环境做任何的改造2）、定位精度满足绝大部分场景应用3）、配套相应的操作软件，易学易用4）、变更使用场景简单，客户自由操作5）、环境亮度要求低，日常光亮环境、昏暗环境皆可作业6）、无惧磁干扰、多金属环境2，TLSAM定位系统优势1），对比相关开源项目Cartographe

4、rHectorslamGmappingGmapping 是基于粒子滤波的SLAM算法，而Cartographer 和Hectorslam 都是基于非线性优化的，现在已经很少用滤波方法来做SLAM 了，绝大多数还 是使用最小二乘法。Cartographer 是有后端的SLAM算法，而Hectorslam是无后端的，前者 更适用于以建图为重点的应用，后者更适合以定位为重点的应用。Cartographer 在无人驾驶平台上会遇到如下问题：（1）定位噪声大：在载具不动的情况下，会产生10cm左右漂移。（2）定位错误：由于载具运动会在快速移动或者大角度倾斜移动的情景下会 产生匹配错误，在融合了 imu的

5、情况下坐标偏差达到10m以上。（3）效率太低：比如在机载飞控板卡上跑cartographer 时常达到400%的cpu 负载（一个核是100%）（4）通讯受阻：目前使用ROS通信方式为Cartographer为通信框架，ROS属于实验性验 证性工具框架，在ROS topic话题中使用TCP本地传输数据，数据从用户层 进入内核，然后再回到用户层，造成巨大延迟。不符合实时要求，会造成通 讯延迟，稳定性较差。该项目优化建立新的通讯方式，直接使用指针传递函 数。数据通信几乎实现零延时效果。2），对比算法进行优化达到效果（1）优化相关算法实现准确的同步定位，累计误差减小。（2）针对载具姿态变化问题做出算

6、法研究，加入融合ekf滤波以后的高精度imu数据做相 关数据处理，让载具达到至少在30度倾斜角时，仍然良好建图定位效果。（3）专门对激光匹配做出优化算法，让载具在速度很快的情况下也能良好定位，能满足正 常大机动。（4）传统算法很多冗余的不分，需要对算法实现减法处理抛弃不需要的组件，让框架轻简， 能挺高运算速率。（5）抛弃ros通信，实现简洁通信协议让整个系统的数据链路更加快速。（6）实现了对位姿先验的支持，可以不限于使用激光测距来实现定位，同其他SLAM算法 一样，使用轮式里程计，视觉里程计，加速度计均可以作为SLAM匹配器的先验，让SLAM 更精确，可以排除大量的匹配错误，让SALM更平滑。

7、（7）完成对后验位姿的准确度判定算法升级，如果判定得到有明显的匹配错误，则丢弃当 前帧定位与建图，明显减少了在场景特征稀少的区域出现位姿发散的问题。3, Tslam构成组件1），点云滤波tslam滤波方法是结合了体素滤波和半径滤波的，首先我们要改变半径体 素滤波的规则，我们不再以固定半径的立方体格子作为滤波单位，转而使用 变化的半径，考虑激光雷达的特性，最好是以弧度与半径作为单位格：类似上图的蛛网状扇形切分个数作为一个参数，一圈一圈的半径切分个数 也作为一个参数。以切分出的扇区作为滤波单元，好处显而易见，当激光雷 达扫的很远的时候格子体积自然而然的增大了。然后再使用半径滤波的思想， 如果某个扇

8、区内的点云个数大于设定的阈值，则保留，也不再取几何中心点 代替，如果小于，我们直接剔除。这种方法在参数设置合理的情况下几乎可 以完美的剔除95%以上的离群点。.激光雷达除畸变在机器人运动过程中，每个激光点都在不同的基准位姿上产生。激光扫描时伴随着机器人的运动，每个角度的激光数据都不是瞬时获得的，当激光雷达扫描 的频率比较低的时候，机器人运动带来的激光帧的运动畸变是不能被忽略的。例如扫描频率 是5Hz的激光雷达，一帧数据的收尾时间差是200ms，如果机器人以0.5m/s的速度沿着x 方向行走并扫描前面的墙体，那么200ms后尾部的测量距离和首部的测量距离在x方向上 就差10cm。所以如果不是高频

9、扫描，这种运动畸变是不容忽视的。去除激光雷达运动畸变的原理是把一帧激光雷达数据的每个激光点对应的激光雷达坐标转 换到不同时刻的机器人里程计上由于imu adis16488的加入，我们可以把它作为一个高精度的惯性里程计使用，我们可以得 到一个搞到1khz的位姿惯导，将激光雷达的每一个激光点，都精确的定位到真实的物理坐 标中。流程：已知当前激光帧的起始时间t1两个激光束之间的时间间隔t惯性里程计数据按照时间顺序存储在一个队列中求解当前帧激光数据中的每一个激光点对应的里程计数据（机器人位姿）根据求解的位姿把所有的激光点转换到同一坐标系下重新封装成一帧激光数据发布出去经过此流程以后，得到的激光点云的畸

10、变被控制在了以往的1%以内，并且，使用的惯性传 感器越好，效果越大，实测使用adis16448去除畸变后，飞行载具3m/s的速度运动，对建 图、定位，均无任何副作用。3），点云匹配Tslam通过对获取到的点云建立误差函数进行点云匹配i-i若将图形xy平面看做姿态位置维度Z轴看做误差维度（z轴越往上误差越小）， 误差函数在三维空间中具有如下属性：若求得尖峰的xy坐标即可求得最小误差的位置和姿态。Tslam选择高斯-牛顿方法解这个最小二乘问题，可以达到足够高的精度和足 够快的运算速度，实测匹配速率高达60帧/秒。4)，网格插值全新的算法来解决定位漂移和大速度下匹配失败的问题，进行网格插值， car

11、tographer 和hectorslam 使用的网格插值法，hectorslam 为了更高的效 率选择了双线性插值，cartographer为了更好的效果使用了双三次插值。效 果并不是特别好。本项目使用插值权重来完成插值，能很好的改善定位漂移相关效果。计算公式： weight = alpha / (x2 + alpha)数据结果：可以看出当alpha取0.005时，x每增加一点点，y值急速下降，y值无线趋近与0,但永远不等于0。这种方式增加了匹配成功的概率和匹配的精度。5）， tiny_ekfTslam ekf组件确保无人机工作在高频定位信息状态。若只使用激光雷达产生定位源，显然无法满足无人

12、机飞行所需要的定位频率，tslam使 用高精度惯导adis16488结合tiny_ekf组件产生高达1khz的定位定姿信息。Tiny_ekf与核心slam模块异步工作，采用递归优化的策略，互相约束，产生高精度的 定位，工作时序分为以下几个步骤：. imu预积分阶段，这一阶段，tinyekf不断获取imu数据，使用毕卡发进行四元数积 分，同时使用加速度计数据进行位置的线性积分，并同步进行所有状态变量协方差的推演.将所有计算出的位姿信息进行放入ekf-delay管道，转入历史融合阶段，这一阶段 保证可以融合历史数据，因为激光雷达，激光测距，气压计等传感器都会有数据延迟，而imu 却几乎没有延迟，为

13、了能够将所有时间轴对齐，不得不缓存imu历史数据。.将预积分后，并经过第二阶段修正了时间的位姿数据，投递到slam模块，作为最 小二乘法的先验，进行点云匹配。这一步由于高精度imu的参与，得到了最为精确的先验。 保证了点云匹配的精准和快速。（积分越准确，运算效率越高）.将点云匹配以后的位置信息作为tinyekf的位置测量值，修正所有状态变量。这一 步就是ekf的测量步骤了，这一步我们可以精确的估计出gyrobias accelbias。.跳转到第一步，再次迭代由于ekf的收敛性，几次迭代下来，gyrobias accelbias将越来越精确，所以积分出的姿态和 位置也越来越精确，slam也将越

14、来越快，越来越准。准确的slam位置投递到ekf测量的时 候，协方差也会越来越小。从而看出，整个系统是一个良性的递归优化过程。下面附上数学建模：6).分离式建图分离式建图是tslam 一个特别的功能，由于slam属于自建自定的特性，位置与地图属 于递归依赖的关系，很难做到万无一失。Tslam分离式建图使用双层地图模式进行定位，底层地图使用上一次保存的完整地图， 上层地图是此次的建图，将两层地图同时映射到一张地图上，作为这一次的全局地图进行定 位。底层地图运行于只读模式无法更改，上层地图可以随着载体的移动，慢慢地进行slam 过程。底层地图一般是使用tslam普通模式，缓慢移动载体采集到的，可由

15、工作人员手持设备 进行采集，由于移动缓慢，我们可以得到最为精确的地图。使用双层地图定位，好处显而易见，就算上层地图由于载具速度过快，或者其他问题， 得到了错误的地图，也不会导致地图的发散，因为，底层的地图具有只读属性，始终约束着 激光帧，就算有10多帧的错误激光数据，也对定位毫无影响。4,基于Linux系统的飞控硬件Alice是一款专为自动驾驶开发者及爱好者打造的自动驾驶控制器，基于linux实时系统，为 二次开发及高级应用扩展提供了友好的环境；支持Ardupilot和PX4固件，支持远程GBD调 试，并提供多种即插即用的开发接口。可支持：多旋翼（3-8轴）、固定翼、直升机、垂直起降固定翼、无

16、人车、无人船、无人潜 水艇。强大的硬件处理能力采用基于NXP Cortex-A9架构的i.MX6Q四核处理器，主频高达1GHz，容量8G，极大的提高 硬件处理能力，例如在运行APM时，CPU使用率仅在10%左右，代码可扩展能力强大。外设丰富，扩展能力强Linux下，传感器驱动、USB驱动等各种支持的外设丰富，可方便的扩展到视觉等高级应用开发。实时计算搭载 Linux PREEMPT_RT，延时低于 1ms# cycli ctest -t 10 - * /dev/cpy_dna_larency pelicyz rifdz 1oadavg:o toSB*51ptoL# cycli ctest -t

17、 10 - * /dev/cpy_dna_larency pelicyz rifdz 1oadavg:o toSB*51ptoL0U5 05 0.56 2/151 11210 1 N 1 4 有 7 E9-El - -ae- -aaabBe cTTTTTTTTTT1112 1111)1114)1115)1116111171118)1119)1120) 1121OOOOOOOO0OEs8s8g&s8ppppppppppI-100G 1:1500- I； 200( I：25OQ I；300Q I:55OQI;400G I：45OQ K：500G I：5500C C c t c c c c c cn

18、rlnrlnrnnrlH41 i 41 Ju 41 JI 41 Ju JI JJmmmmmmmmmm8 rM 4 1 607 了 -H a0-5233 7 566317di2o9B7ttttttttctccr-cccr-cccAAAAAAAAAAoooooooooo2 2 2 2 2 2 2 27-223 Avg22 AvgAvg24 Avg23 Avg2:3 Avg29 Avg24 Avg24 Avgzz Avg2424 Max24 Max23 Max24 Max24 Max24 Max23 Max24 Max2fi Max116 7677 5156 4342 SI41 74支持远程GDB调

19、试（软硬件断点，内存断点，单步步入、步过、跳出），VS 远程开发在Visual Studio开发环境下，实现远程附加调试（例如在使用APM或者PX4下，不用再搭 建开发环境，VS下可直接开发）ihrii_upd*ij卜01台小 if fspafftartu?4 =AFjTFSi iGF5_M_Fn_IDi 1 re Muid ihrii_upd*ij卜01台小 if fspafftartu?4 =AFjTFSi iGF5_M_Fn_IDi 1 re Muid naad bIitt. r rairdxE-i-fldC：kk hrtlyit T-x, tIdfl-I jr. yl ;it 1 z：

20、r-t_hww_ro_GUrflnrlDsi imd f / Ljnxfffl thL2 tlLlufd- m-Ju grf U h- riwZy EvTbit -=irpLn0 1ih EKF jt i. xp4d -dvt* :nwUhi口lR veLE；f hflh-awll/E-Fai 值.p - ihy*d-c*础乜 LA, 支持SFTP文件传输使用SFTP，全图形化管理板卡的根文件系统boot192.168A371O1 x过短homelost-l-foundmn-mntmedia- etchomelibboot192.168A371O1 x过短homelost-l-foundmn-

21、mntmedia- etchomelib- lost+found- media dXWKT-JKT-K巨。勺后Thu l-Jazi-19700S:Q4siahome(S)* drwxT-KT-x409Thu01-Jan-197008:02:00sbin(S)才1胆24晚依丁今隋藏I的proc支持远程桌面基于SSH的远程图形界面支持wifi通讯，4G/5G扩展可实现WiFi图数传功能产品参数基本参数主处理器NXP i.MX6Q 四核协处理器STM32F405架构Cortex-A9主频1GHz内存1GB DDRROM8GB eMMC操作系统Linux-RT接口引出I2C1路USB HOST3 路（

22、2 路为 gh1.25-4p 接口, 1 路为 Micro-usb 接口）USB OTG1 路（Micro-usb 2.0 接口）UART3路（包含一个可做debug调试串口）pwm12路（标准8个+扩展4个）s.bus1路adc2路Can1路板载传感器气压计MS5611加速度计/陀螺仪MPU6050磁力计QMC5883LRGBLED1个格尺寸170*73*16（mm）电源输入DC5V 2A额定功率4W环境温度050重量97g（铝壳）5,实现自主路径探索规划导航软件VNAV是一款自主设计的高实时性、高效率的投影匹配地面端操作软件，结合我们自主研 发的ALICE自驾仪控制器，已经实现了实时操作和显示激光建模、避障和最优路径自主 规划的功能。以下为软件界面展示vnav viewer回r r ._ _0 bytes/seconGPSO LOCK1参数名值*CA MJSOUTDOORMODE1CAMJS_NEAR_MODE01 1CAMJNT