一种基于语义VSLAM的无人机导航方法

一种基于语义vslam的无人机导航方法
技术领域
1.本发明属于计算机室内定位技术领域，具体涉及一种基于语义vslam的无人机导航方法。


背景技术：

2.无人机作为机器人的一种，广泛应用于农作物检测、交通运输、物流配送、自然资源的勘探、智慧城市应用等领域。
3.现在无人机的室外自动定位系统多数采用global positioning system(gps)，甚至超高精度的real
‑
time kinematic(rtk)，所以无人机在室外可以达到较好的定位导航效果。但在室内或者没有gps的地方，则需要依靠其他方式来进行定位导航。传统的室内定位方案有超宽带技术、蓝牙、二维码、射频识别、光学捕捉系统等，以及现在较为先进的激光雷达即时定位与地图构建(slam)和视觉即时定位与地图构建(vslam)。传统的定位方式只能提供一个相对的坐标位置用于定位导航，而激光雷达slam产生的地图多数是2d栅格图，只能应用于一个平面上的定位导航，但是多维度的激光雷达成本非常昂贵，且室内环境复杂，存在低光照及物体摆放复杂等情况。
4.现在slam应用最广的是扫地机器人，利用二维激光对周围环境进行扫描，并建立可以导航的二维栅格图。因为二维激光所建立的地图不具备语义，而且只能做简单的避障导航，局限了机器人对环境的感知，所以语义的应用有利于解决室内环境或者非室内环境的定位导航以及测绘方面的应用，并且可以做到智能理解周围环境。


技术实现要素：

5.本发明提出一种基于语义vslam的无人机导航方法，本发明的主要功能分为可视化地图构建、自主导航和目标识别、定点定高、航迹飞行。
6.本发明的主要实现步骤如下：
7.步骤1、lime低光照优化处理
8.利用rgb
‑
d传感器获取rgb图和深度图，初始化输入图像的通道数，将图像数值归一化到0～1。当输入为rgb图像时，筛选出rgb三个通道中最大的那个值，接下来进行伽马校正，然后利用retinex进行求解计算增强后的rgb值，并进行阈值化去噪处理，最后保存输出。
9.步骤2、八叉树地图的生成
10.vslam利用步骤一处理后的rgb图和深度图进三维重构，生成三维点云图及关键帧，yolov4
‑
tiny利用步骤一处理后的rgb图像进行语义标注，每隔一段时间利用超体素分割在三维点云图上进行预分割，生成纹理、材质及颜色相似度高于设定阈值的点云，聚在一起得到新的三维点云图，将关键帧中的二维语义标注映射到新的三维点云图，形成语义地图。vslam将语义地图转换为八叉树地图。
11.步骤3、轨迹优化
12.对fast
‑
planner进行参数初始化，计算障碍物相关代价。遍历所有局部点云，提取障碍物三维坐标，将本地点云转化成全局点云。将障碍物坐标进行判断，计算出安全距离，防止飞控逾越安全距离，完成一个点云的判断以后继续遍历，直至遍历完成所有局部点云。遍历完成后寻找代价最小的路径，即最优路径，并提取最优路径的航向角。
13.步骤4、航迹飞行
14.无人机利用飞行控制软件订阅任务模块、控制指令及无人机状态等话题，设置发布期望姿态，并初始化参数包括起飞高度、安全高度、下降速度及地理围栏。初始化控制器类和通讯类，控制控制器并与px4通信；提取步骤3中最优路径的航向角进行航迹飞行。
15.作为优选，所述的利用超体素分割在三维点云图上进行预分割，生成纹理、材质及颜色相似度高于设定阈值的点云，聚在一起得到新的三维点云图，将关键帧中的二维语义标注映射到新的三维点云图，形成语义地图；具体为：
16.首先超体素分割算法初始化，包括点云变量初始化、加载点云数据、禁止单视角转换、通过设置体素尺寸大小来决定底层八叉树的叶子尺寸，通过设置种子大小决定超体素的大小、颜色权重、设置空间距离权重，初始化超体素存储容器，用于超体素存储；再计算超体素质心，并对原始点云进行下采样，计算超体素法向量之后构造邻接图进行图计算；遍历容器获取容器中的标签值，将标签值和超体素建立对应关系，从而构造纹理、材质及颜色类似的点云集，并绘制该超体素与其相邻超体素的子图得到新的三维点云图，指向下一个标签，判断容器不为空的话，继续遍历容器，否则表示超体素分割完成，遍历所有超体素，并取出关键帧中对应的lable，将相应的点云团进行标注，融合生成语义地图。
17.作为优选，所述的yolov4
‑
tiny利用步骤一处理后的rgb图像进行语义标注，具体如下：
18.首先rgb图像上增加灰条，实现不失真的图片大小调整，再加上batch_size的维度，接下来使用yolov4
‑
tiny进行算法识别，将处理后的图片输入到网络中去进行预测，预测之后会出现很多预测框堆叠，使用非极大值抑制；如果说在图片中没有检测出相关目标，则返回原图，反之检测到目标时，所处理过后的预测框进行得分筛选，并且去掉灰条，将图片中的目标物体进行框定标注。
19.本发明相当于现有技术的有益效果为：
20.1、结合lime光度增强算法对输入图像进行预处理，使得slam系统在低光照环境下减少精度丢失，更加鲁棒。
21.2、在轻量级神经网络yolov4
‑
tiny的基础上结合数据增强、迁移学习、初始学习率设置及余弦学习率调整策略进行优化，以进一步提升网络模型精度及泛化能力。经过训练优化，提升网络模型的平均精度。
22.3、系统结合超体素预分割三维稠密点云，再将二维物体标注映射到分割后的点云上较高效率地实现三维标注形成语义地图。
23.4、结合局部规划算法、全局规划算法和fast
‑
planner算法进行轨迹优化，以适应复杂环境及增加规划速度。
附图说明
24.图1为系统框图；
25.图2为硬件框架图；
26.图3为软件整体架构；
27.图4为lime优化软件流程图；
28.图5为yolov4
‑
tiny软件设计流程图；
29.图6为超体素分割及地图融合软件设计流程图；
30.图7为路径规划流程图；
31.图8为飞行控制软件设计流程图。
具体实施方式
32.下面结合附图和具体实施方式对本发明作进一步的说明。
33.如图1所示，本发明分为应用层、算法层、通信层、软件驱动层和硬件层五个部分，应用层为本发明所实现的现有功能，设计时考虑可拓展性，留有软件扩展接口，可在本发明上研发其他功能，主要功能有可视化地图构建、自主导航和目标识别、定点定高、航迹飞行等；算法层为本发明的核心层，分为上层感知算法层与下层控制算法层。上层主要包括目标检测卷积神经网络yolov4
‑
tiny，路径规划算法fast
‑
planner、a*、vfh、apf，运动估计及地图构建算法orb
‑
slam3；下层主要由控制算法串级pid，传感器数据处理数据滤波算法扩展卡尔曼、姿态解算等组成；通信层为各个模块之间的通信机制，软件算法之间的通信主要为ros消息机制、上层感知算法层与下层控制层之间为mavros及mavlink通信、各个传感器之间的协议通信等；软件系统层包括上层感知算法层和下层控制算法层，上层算法层运行于图形化操作系统ubuntu，下层算法层运行于实时嵌入式操作系统nuttx。软件驱动层主要为各传感器与主机之间的通信驱动包括uart、usb、i2c、spi、摄像头驱动等；硬件层主要由主机和传感器组成，主机为i7 8565u及飞控硬件平台pixhawk，传感器为双目深度摄像头d435i、imu等。
34.如图2所示，基于语义vslam的无人机导航系统，主要包括电源模块、pixhawk、i7 8565u、d435i、imu和电调等。本发明主要电源为6s 22v电源，降压为三种电压，分别为19v,5v,3.3v。其中19v为工控机i7 8565u供电，3.3v为imu模块供电，5v为pixhawk供电，电调直接由总电源供电，无需降压。d435i通过usb3.0直接与工控机相连，由工控机直接供电，并与工控机进行通信，完成图像传输。pixhawk读取imu数据，并与工控机进行交互，算法计算结果转换为控制信号传输给电调，进行飞控飞行控制。
35.如图3所示，总软件设计主要分为感知算法和控制算法两部分，本发明采用多线程设计架构，集运动计算、建图、导航及可视化于一体，具体架构如下：
36.感知算法部分主要基于硬件平台i7
‑
8565u和软件系统ubuntu16.04运行，首先由双目深度摄像机d435i提供rgb图像及深度图，orb
‑
slam3根据rgb图像及深度图进行运动估计及三维点云地图构建，每隔一段时间利用超体素分割算法将全局点云进行体素分割，生成纹理相近点云聚在一起的三维点云，有利于后面分割。另外yolov4
‑
tiny利用rgb图像进行语义标注，将同一物体的像素，在vslam的帧中都进行标注并结合超体素分割后的结果，经过vslam的三维映射生成语义地图。完成局部语义地图的同时转换成导航需要的八叉树地图，通过fast
‑
planner算法在八叉树地图上进行路径规划，并将规划结果通过mavros发送给控制模块。
37.控制算法主要基于硬件平台pixhawk和实时嵌入式软件系统nuttx运行，接受由感知算法部分传过来的数据，并将数据从enu惯性系转换为ned惯性系，实时读取imu数据，并结合规划路径进行姿态和位置控制，利用串级pid实时计算出pwm信号，传送给电调，按照规划航迹进行飞行。
38.如图4所示，本发明采用lime算法进行场景低光照优化处理，以进一步提高系统精度及鲁棒性。首先初始化输入图像的通道数，然后将图像数值归一化到0～1。通过不同的通道数进行处理，输入rgb图像时，筛选出rgb三个通道中最大的那个值，接下来进行伽马校正，然后利用retinex进行求解计算增强后的rgb值，并进行阈值化去噪处理，最后保存输出。
39.如图5所示，yolov4
‑
tiny算法的首先将相关变量初始化，并获得所有分类和先验框。再建立yolov4
‑
tiny模型，实例化相关类，载入训练后的权重值，并建立特征层解码所要使用的工具和设置不同的画框颜色。上述流程均属于算法初始化，初始化完成之后，开始进行检测，首先在图片上增加灰条，实现不失真的图片大小调整，再加上batch_size的维度，接下来使用yolov4
‑
tiny进行算法识别，将图片输入到网络中去进行预测，预测之后会出现很多预测框堆叠，使用非极大抑制。如果说在图片中没有检测出相关目标，则返回原图，反之检测到目标时，所处理过后的预测框进行得分筛选，并且去掉灰条，一方面将图片中的目标物体进行框定标注，用作可视化，另一方面将框定的这部分长宽和坐标点存储到容器，用作2d图像到3d点云的映射范围。
40.如图6所示，在进行三维映射之前将点云进行超体素分割，使得语义识别更加准确。首先进行算法初始化，包括点云变量初始化、加载点云数据、禁止单视角转换、通过设置体素尺寸大小来决定底层八叉树的叶子尺寸，通过设置种子大小决定超级体素的大小、颜色权重、设置空间距离权重。初始化超体素存储容器，用于超体素存储。再计算超体素质心，并对原始点云进行下采样，计算超体素法向量之后构造邻接图便于进行图计算。遍历容器获取容器中的标签值，将标签值和超体素建立对应关系，从而构造纹理、材质及颜色类似的点云集，并绘制该超体素与其相邻超体素的子图，并指向下一个标签，判断容器不为空的话，继续遍历容器，否则表示超体素分割完成，在此基础上融合生成语义地图。遍历所有超体素，并取出关键帧中对应的lable，将相应的点云团进行标注，并使用不同颜色可视化显示。
41.如图7所示，本发明的路径规划设计主要有全局规划器、局部规划器、fast
‑
planner。全局规划器软件设计首先读取各项参数，定高和设置安全距离，使得飞机安全飞行，加载地图进行路径规划。订阅目标点及无人机状态参数，发布路径指令，设置路径规划器执行周期，并周期性执行路径规划循环，路径追踪循环。a*算法初始化，规划器状态参数初始化，获取时间戳，随后设置相关参数，进行起飞；局部规划器首先进行参数读取，设置高度及最大速度，防止飞行过快产生安全问题。同时订阅所要达到的目标点、无人机状态及点云数据，完成订阅之后发布期望速度，并设置执行规划的定时器，频率为1hz，即一秒执行一次局部规划；完成规划后进行控制飞行，设置飞行控制时间，一秒四次，再确定局部规划算法使用apf还是vfh。所有参数及执行函数设置完成后使用ros消息机制发布初始化参数、起飞标志位设置及地图初始化，最后等待ros节点回调。vfh算法软件设计进行参数初始化，包括感知障碍物距离、目标权重值、直方图个数、障碍物权重及安全停止距离。然后判断局部
地图及里程计是否在容器中，如果没有准备好直接结束程序，反之获取时间戳，并计算障碍物相关代价。接下来遍历所有局部点云，提取障碍物三维坐标，并将坐标进行坐标系转换，即将本地点云转化成全局点云。将障碍物坐标进行判断，如果不在传感器感知范围内则继续遍历下一个点云，否则进行归一化障碍物距离，并计算出安全距离，防止飞控逾越安全距离，计算出总代价并存储下来，完成一个点云的判断以后继续遍历，直至遍历完成所有局部点云。获取当前目标点位置坐标，遍历初始化的180个直方图，计算角度差代价，当前的角度与目标角度差越多，该代价值越大，最后将代价值进行累加，接着遍历。遍历完成后寻找代价最小的路径，即最优路径，并提取最优路径的航向角，到此完成了一次整体局部规划，判断是否规划成功，设置不同的标志位并返回标志位。
42.如图8所示，本发明的飞行控制软件首先订阅任务模块、控制指令及无人机状态等话题，设置发布期望姿态，并初始化参数包括起飞高度、安全高度、下降速度及地理围栏。初始化控制器类和通讯类，用于后面控制器控制及与px4通信。将飞机的初始电机状态置为idle模式，使得电机进入怠速旋转，即电机都旋转起来准备起飞。然后是记录时间点，为pid控制器积分和微分做准备。完成初始化之后，使用回调函数进行程序节点回调，根据当前模式来执行，分别有idle模式(准备模式)、takeoff模式(起飞模式)、hold模式(悬停模式)、land模式(降落模式)及move模式(飞机移动模式)。当前状态为idle模式时，四个电机速度都设置成相同缓慢速度，准备起飞。当前状态为takeoff模式时，设置起飞位置，并设置坐标系为enu。当前状态为hold模式时，将当前位置设置成目标位置，进行位置控制。当前状态为land模式时，设置下降速度，使得飞机进行下降。状态程序执行完成后，执行串级pid控制，输出电机油门值，并发送期望姿态。
43.以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员，在不脱离本发明构思的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明保护范围内。