一种自主定位式无人机及其室内路径自主规划方法与流程

1.本发明涉及无人机自主定位技术领域，特别涉及一种自主定位式无人机及其室内路径自主规划方法。


背景技术：

2.随着社会进步与发展，室内移动智能机器人成为目前机器人的热门研究方向。自主定位与导航系统设计是实现室内移动机器人自动化的关键技术。定位精度及导航效率决定了机器人的推广应用程度。但室内环境存在gps缺失或盲点、物体布局复杂、动态因素多、部分环境纹理缺失、光照强度易随动态物体移动发生变化等问题，限制了移动机器人在家庭、车间等室内环境中的应用，因此有必要提出一种具有室内路径自主规划的自主定位式机器人。


技术实现要素：

3.本发明提供了一种自主定位式无人机及其室内路径自主规划方法，在无人机主体上设置激光雷达、双目摄像头和惯性传感器，通过惯性传感器实时解析无人机的位置信息，基于提前规划的全局路径，结合激光雷达和双目摄像头实时探测，建立环境地图并绕过障碍物，实时更新全局路径。
4.本发明的技术方案是：
5.一种自主定位式无人机，包括：
6.无人机本体；
7.激光雷达，其连接无人机主体，以获取环境信息；
8.双目摄像头，其连接无人机主体，以获取地面信息；
9.惯性传感器，其连接无人机主体，以获取无人机位姿信息；
10.自主定位模块，其与激光雷达、双目摄像头和惯性传感器通信连接，以解析无人机的位置信息，并创建环境地图；
11.障碍物识别和路径规划模块，其连接自主定位模块，能够进行障碍物识别、全局和局部路径规划。
12.优选的是，惯性传感器包括三轴陀螺仪和三轴加速度计。
13.一种自主定位式无人机室内路径自主规划方法，基于上述的自主定位式无人机，包括：
14.步骤一、确定无人机起点和目标点，并根据室内环境参考图规划全局路径；
15.步骤二、实时获取激光雷达、双目摄像头和惯性传感器的探测信息，解析无人机的位置信息，并创建环境地图；
16.步骤三、预判当前节点到下一节点飞行过程是否存在障碍物，若存在，进行障碍物识别和特征提取；
17.步骤四、根据障碍物的特征信息，规划局部路径，并更新全局路径和环境地图。
18.优选的是，步骤一包括：
19.将室内环境参考图进行栅格化，并对参考图中的障碍物进行膨胀处理；
20.将不含障碍物的网格标记为自由栅格，含有障碍物的栅格标记为障碍栅格，并划分为自由栅格区和障碍栅格区；
21.将紧邻障碍栅格区的自由栅格作为顶点，初始化无人机起点与各顶点的距离；
22.向外层层扩展，循环搜索，直到起点与各顶点的距离最短。
23.优选的是，解析无人机的位置信息包括：
24.无人机本体按照全局路线行进，获取惯性传感器的探测信息，利用惯性积分方法连续计算飞行的相对距离和航向；
25.根据起点位置和航向计算当前无人机的绝对位置及方位：
26.基于卡尔曼滤波模型，建立航迹推算状态方程，并根据状态方程，计算误差状态向量和速度测量值误差；
27.结合贝叶斯滤波算法，以全局路径约束航迹推算轨迹。
28.优选的是，障碍物识别包括：
29.获取激光雷达的采集特征图，将特征图分割为多个相等的方形区块；
30.分别计算每个方形区块的占比值，将占比值大于占比阈值的方形区块作为感兴趣区块；
31.设定邻域和最低密度值，遍历感兴趣区块，得到每个感兴趣区块的中心点；
32.若相邻感兴趣区块的中心点密度相连，则相邻感兴趣区块归为一个障碍物团簇。
33.优选的是，特征提取包括：
34.采用卷积神经网络训练得到分类模型；
35.获取激光雷达和双目摄像头采集的特征图，将三维锚框投影到特征图中；
36.根据投影结果将特征图裁剪成多个二维锚框；
37.将二维锚框输入到分类模型中进行目标分类和边界框回归。
38.优选的是，二维锚框尺度为8、16、32中的一种，比例为2:1、1:1、1:2中的一种。
39.优选的是，局部路径包括：
40.基于无人机的位置信息，计算无人机所受的势场合力；
41.判断无人机是否处于局部极小值；
42.如果是，无人机沿斥力势场等势线绕行，直到逃离局部极小值区域。
43.优选的是，局部极小值包括：无人机所受势场合力趋于零和或无人机行进位移与路程比趋于零。
44.本发明的有益效果是：
45.1、本发明提供的自主定位式无人机及其室内路径自主规划方法，在无人机主体上设置激光雷达、双目摄像头和惯性传感器，通过惯性传感器实时解析无人机的位置信息，基于提前规划的全局路径，结合激光雷达和双目摄像头实时探测，建立环境地图并绕过障碍物，实时更新全局路径，适用于gps缺失的室内环境。
46.2、本发明提供的自主定位式无人机室内路径自主规划方法，基于卡尔曼滤波模型结合贝叶斯滤波算法，解析得到无人机的位置信息，自主定位精度高。
47.3、本发明提供的自主定位式无人机室内路径自主规划方法，基于卷积神经网络建
立二维分类模型，进行目标分类和边界框回归，障碍物特征提取高效，准确率高，同时采用人工势场法规划局部路径，能够快速绕过障碍物。
附图说明
48.图1为本发明提供的自主定位式无人机的结构示意图。
49.图2为本发明提供的自主定位式无人机室内路径自主规划方法流程图。
50.图3为本发明的一个实施例中制定全局路径的方法流程图。
51.图4为本发明的一个实施例中障碍物膨胀前的栅格图。
52.图5为本发明的一个实施例中障碍物膨胀后的栅格图。
53.图6为本发明的一个实施例中循环搜索的原理图。
54.图7为本发明的一个实施例中解析无人机的位置信息的方法流程图。
55.图8为本发明的一个实施例中无人机飞行图。
56.图9为本发明的一个实施例中障碍物识别和特征提取方法流程图。
57.图10为本发明的一个实施例中局部路径规划方法流程图。
具体实施方式
58.以下由特定的具体实施例说明本发明的实施方式，熟悉此技术的人士可由本说明书所揭露的内容轻易地了解本发明的其他优点及功效，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
59.需要说明的是，在本发明的描述中，术语“中”、“上”、“下”、“横”、“内”等指示的方向或位置关系的术语是基于附图所示的方向或位置关系，这仅仅是为了便于描述，而不是指示或暗示装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。
60.此外，还需要说明的是，在本发明的描述中，除非另有明确的规定和限定，术语“设置”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域技术人员而言，可根据具体情况理解上述术语在本发明中的具体含义。
61.如图1所示，自主定位式无人机，包括无人机本体110、激光雷达120、双目摄像头130、惯性传感器140、自主定位模块150和障碍物识别和路径规划模块160。
62.其中，激光雷达120连接无人机主体110，以获取环境信息，双目摄像头130连接无人机主体110，以获取地面信息，惯性传感器140连接无人机主体110，以获取无人机位姿信息，自主定位模块150与激光雷达120、双目摄像头130和惯性传感器140通信连接，以解析无人机的位置信息，并创建环境地图，障碍物识别和路径规划模块160连接自主定位模块150，能够进行障碍物识别、全局和局部路径规划。
63.进一步的，惯性传感器140包括三轴陀螺仪141和三轴加速度计142。
64.基于上述自主定位式无人机的室内路径自主规划方法，如图2所示包括：
65.s110、确定无人机起点和目标点，并根据室内环境参考图规划全局路径；
66.s120、实时获取激光雷达、双目摄像头和惯性传感器的探测信息，解析无人机的位置信息，并创建环境地图；
67.s130、预判当前节点到下一节点飞行过程是否存在障碍物，若存在，进行障碍物识别和特征提取；
68.s140、根据障碍物的特征信息，规划局部路径，并更新全局路径和环境地图。
69.本发明通过惯性传感器实时解析无人机的位置信息，基于提前规划的全局路径，结合激光雷达和双目摄像头实时探测，建立环境地图并绕过障碍物，实时更新全局路径，适用于gps缺失的室内环境，自主定位精度高，构建高精度环境地图，还可规划局部最优路径并快速绕过障碍物。
70.获取室内环境的结构图，如建筑结构或装修结构图，能够对无人机飞行区域做初步预判，绘制全局地图，能够有效约束无人机飞行路线，如图3所示，具体包括：
71.s111、将室内环境参考图进行栅格化，并对参考图中的障碍物进行膨胀处理。
72.s112、将不含障碍物的网格标记为自由栅格，含有障碍物的栅格标记为障碍栅格，并划分为自由栅格区和障碍栅格区；
73.对环境参考图进行图像处理，转化为计算机能够识别的栅格化模型，利用栅格块组成环境参考图中的可行区域与不可行区域，其中可以自由飞行且在此区域内不含任何障碍物的网格称为自由栅格，含有障碍物的栅格称为障碍栅格，中除了自由栅格与障碍栅格外还存在第三种栅格，该栅格内一部分是自由区域，另一部分是障碍物区域。为此，按照图像形态学扩充理论对其进行障碍物膨胀处理，当障碍物占据某一栅格时，无论占据栅格的面积为多少均将其视为完全不可行区域。
74.s113、将紧邻障碍栅格区的自由栅格作为顶点，初始化无人机起点与各顶点的距离；
75.s114、向外层层扩展，循环搜索，直到起点与各顶点的距离最短。
76.向外层拓展时，每加入一个新节点，需要对边进行松弛判断，判断起点通过新顶点到达其他顶点的距离是否比起点直接到达其他节点的距离小，若小则替换起点与其他节点间的直达路径。
77.在一个具体实施例中，障碍物膨胀前的栅格图如图4所示，膨胀后的栅格图如图5所示，循环搜索的原理如图6所示，获得的路径更短、转折角更小、转向更平滑，能够更好满足无人机实际飞行的要求。
78.如图7所示，解析无人机的位置信息包括：
79.s121、无人机本体按照全局路线行进，获取惯性传感器的探测信息，利用惯性积分方法连续计算飞行的相对距离和航向，如图8所示。
80.s122、根据起点位置和航向计算当前无人机的绝对位置及方位。
[0081][0082]
其中，t表示采样时间；z
t
表示t时刻传感器坐标系到导航坐标系的转换矩阵，e表
示单位矩阵，t表示采样单位时间，ω表示三轴角速度反对称矩阵，v
t
表示是t时刻导航坐标系下的速度，f
t
表示是传感器坐标系下加速度计的比力测量值，g表示导航坐标系下的重力加速度，p
t
表示t时刻计算得到的导航系下的位置。
[0083]
s123、基于卡尔曼滤波模型，建立航迹推算状态方程，并根据状态方程，计算误差状态向量和速度测量值误差。
[0084]
航迹推算状态方程为：
[0085][0086]
其中，表示姿态误差，表示姿态误差估计，δv表示速度误差，表示速度误差估计，δp表示位置误差，表示位置误差估计，δw
i
表示陀螺仪测量误差，δf表示加速度计测量误差。
[0087]
根据所状态方程，计算误差状态向量和速度测量值误差；
[0088][0089]
其中，f
t
表示t时刻的状态转移矩阵，h
v
表示基于速度的测量矩阵，w
t
‑1表示状态噪声，η
v,t
表示速度测量噪声，z
v,t
表示速度测量值对应的速度误差，状态矩阵f
t
和速度测量矩阵h
v
表示如下：
[0090][0091]
h
v
＝[o3×3i3×3o3×3]；
[0092]
这里，δt表示采样间隔，i3×3表示3
×
3的实阵；o3×3表示3
×
3的零阵，s
t
表示比力向量f的反对称矩阵；
[0093]
s124、结合贝叶斯滤波算法，以全局路径约束航迹推算轨迹。
[0094]
建立约束估计模型为：
[0095][0096]
其中，x
t
表示状态向量，z
t
表示测量向量，w表示过程噪声，v表示测量噪声，函数f和h是全局路径的非线性约束函数。
[0097]
基于卡尔曼滤波模型结合贝叶斯滤波算法，解析得到无人机的位置信息，自主定位精度高。
[0098]
如图9所示，障碍物识别和特征提取包括：
[0099]
s131、获取激光雷达的采集特征图，将特征图分割为多个相等的方形区块。
[0100]
s132、分别计算每个方形区块的占比值，将占比值大于占比阈值的方形区块作为感兴趣区块。
[0101]
s133、设定邻域和最低密度值，遍历感兴趣区块，得到每个感兴趣区块的中心点。
[0102]
s134、若相邻感兴趣区块的中心点密度相连，则相邻感兴趣区块归为一个障碍物团簇。
[0103]
s135、采用卷积神经网络训练得到分类模型。
[0104]
s136、获取激光雷达和双目摄像头采集的特征图，将三维锚框投影到特征图中。
[0105]
本发明设置了3种尺寸的锚框，锚框由其中心坐标(x,y,z)和长度，宽度、高度(l,w,h)六个参数表示，锚框的大小通过对数据集中检测目标的标签信息进行聚类来确定。由于雷达点云稀疏会导致许多空锚，对于不包含点云的空锚，根据锚框中点云的总和是否为零来决定是否将其剔除。
[0106]
s137、根据投影结果将特征图裁剪成多个二维锚框。
[0107]
为了实现雷达点云和图像的特征融合，需要对特征图进行roi池化。因此，本发明将三维锚框投影到特征图上，然后对其进行裁剪和尺寸调整，对于三维锚框(x,y,z,l,w,h)，特征图上投影区域的左上角和右下角可以表示为(x
l,left
,z
l,left
)和(x
l,right
,z
l,right
)，即：
[0108][0109]
s138、将二维锚框输入到分类模型中进行目标分类和边界框回归。
[0110]
由于锚框通常不能很好地包围目标障碍物，所以需要通过神经网络进行回归，以帮助网络输出更为准确的边界框。
[0111]
作为一种优选，二维锚框尺度为8、16、32中的一种，比例为2:1、1:1、1:2中的一种。
[0112]
基于卷积神经网络建立二维分类模型，进行目标分类和边界框回归，障碍物特征提取高效，准确率高。
[0113]
本发明局部路径规划采用人工势场法，基本思想是把智能体的运动环境抽象为目标点产生的引力势场和障碍物产生的斥力势场构成的势场空间，在这个空间中，智能体分别受到障碍物的斥力和目标点的引力，并在这两种力的共同作用下向目标点的方向运动，最终到达目标点位置，具体如图10所示，包括：
[0114]
s141基于无人机的位置信息，计算无人机所受的势场合力。
[0115]
假设无人机目标点坐标为p
e
＝(x
e
,y
e
)，那么无人机到位置p
i
＝(x
i
,y
i
)时，引力势场函数为：
[0116][0117]
其中，u
g
(p
i
)表示引力势场函数，k
g
表示引力势场正比例系数，n表示引力势场因子，d(p
i
,p
e
)表示无人机到目标点的距离。
[0118]
斥力势场函数为：
[0119][0120]
其中，u
r
(p
i
)表示斥力势场函数，k
r
表示斥力势场正比例系数，d(p
i
,p
o
)表示无人机与障碍物之间的距离，d
o
表示受障碍物影响的最大距离，当且仅当无人机到d
o
范围内的时候才会受到障碍物斥力作用。
[0121]
无人机飞行路径中一般都要经过多个障碍物，产生的斥力势场也会有多个，所以其受到的斥力也应是多个斥力势场产生斥力的合力∑f
r
(p
i
)。那么，无人机飞行过程中受到的引力与斥力的合力为：
[0122]
f(p
i
)＝f
g
(p
i
) ∑f
r
(p
i
)；
[0123]
其中，f(p
i
)表示无人机在p
i
点受到的势场合力，f
g
(p
i
)表示无人机在p
i
点受到的引力，f
r
(p
i
)表示无人机在p
i
点受到的斥力。
[0124]
s142判断无人机是否处于局部极小值。
[0125]
其中，局部极小值的判断规则为：无人机所受势场合力趋于零和或无人机行进位移与路程比趋于零，即：
[0126]
f(p
i
)＝f
g
(p
i
) ∑f
r
(p
i
)
→
0，和/或
[0127][0128]
其中，|x
i
‑
x
j
|表示i点到j点的直线距离即位移，s表示路程。
[0129]
s143如果是，无人机沿斥力势场等势线绕行，直到逃离局部极小值区域。
[0130]
采用人工势场法规划局部路径，能够快速绕过障碍物。
[0131]
以上内容仅是本发明的实施例，方案中公知的具体结构及特性等常识在此未作过多描述，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不脱离本发明的前提下，还可以作出若干变形和改进，这些也应该视为本发明的保护范围，这些都不会影响本发明实施的效果和专利的实用性。