一种融合语义特征的视觉惯性组合导航方法

摘要

本发明公开了一种融合语义特征的视觉惯性组合导航方法，采集k时刻RGBD视觉传感器数据S(k)、加速度计数据和陀螺仪数据根据视觉传感器数据S(k)利用视觉里程计解算得到相机当前位姿T(k)；利用视觉传感器数据S(k)进行相邻两图像帧之间的语义平面特征提取和匹配；利用惯性传感器数据进行相邻两图像帧之间的预积分；联合语义平面观测残差、视觉里程计相对位姿观测残差和惯性预积分残差优化求解载体导航信息；输出载体导航信息和相机内参。本发明可以有效提高导航系统定位的精确度及鲁棒性。

说明书

技术领域

本发明属于机器人导航技术领域，具体涉及一种融合语义特征的视觉惯性组合导航方法。

背景技术

视觉SLAM算法由于其感知信息的丰富成为机器人自主导航领域中的一大研究热点。传统的视觉SLAM方法提取点、线等特征进行环境的特征描述以及位姿解算，这些特征通过底层亮度关系进行描述匹配，没有充分利用场景中的结构冗余信息。单一的视觉传感器由于感知维度有限，难以满足无人机在室内复杂环境中的鲁棒定位。这些都对导航系统的精度和可靠性造成了影响。

发明内容

针对现有技术的不足，本发明的目的在于提供一种融合语义特征的视觉惯性组合导航方法，以提高无人机自主定位的精度。

为达到上述目的，本发明的技术方案为：

一种融合语义特征的视觉惯性组合导航方法，包括步骤如下：

步骤1、采集k时刻RGBD视觉传感器数据S(k)、加速度计数据

步骤2、根据视觉传感器数据S(k)利用视觉里程计解算得到相机当前位姿T(k)；

步骤3、基于相邻两图像帧之间的视觉传感器数据，构建语义平面特征图，将当前帧的语义平面与地图中的语义路标进行匹配，用来获取当前关键帧与语义路标间的观测关系；

步骤4、基于惯性传感器数据进行相邻两图像帧之间的惯性预积分，所述惯性传感器数据包括加速度计数据、陀螺仪数据；

步骤5、基于语义平面观测残差、视觉里程计相对位姿观测残差和惯性预积分残差三者之和作为联合的优化函数，对所述优化函数进行非线性优化实现位姿求解。

步骤6、输出载体导航信息和相机内参，并返回步骤1

优选地，获取所述相机当前位姿具体为：首先对相邻两帧提取ORB特征，然后利用两帧之间的ORB特征匹配关系以及pnp解算出两关键帧之间的相对位姿，通过相对位姿的累加即得到k时刻相机的位姿T(k)。

优选地，基于相邻两图像帧之间的视觉传感器数据，获取平面信息，基于所述平面的语义类别、形心、法向以及水平垂直类型，构建语义平面特征：

·s

·s

·

·s

其中，s

基于构建的所述语义平面特征，构建语义平面特征图；

定义初始语义路标，将每帧检测语义平面S

优选地，所述k时刻得到的惯性传感器数据包括从k-1时刻到k时刻的加速度计数据

其中，n

在相邻两图像帧之间有若干惯性传感器数据，采用迭代的方式对相邻两图像帧之间的所有惯性传感器数据进行预积分：

其中，

使视觉数据的频率与惯性预积分频率保持一致，得到相邻两图像帧之间的惯性预积分

优选地，所述优化函数为：

其中，Semantic部分代表语义路标观测的残差，

优选地，所述语义路标观测误差

其中，

计算e关于

所述位姿观测残差

其中，

分别求解出e关于位姿

计算惯性预积分残差

优选地，所述语义路标观测误差中e关于

其中，ξ为位姿

优选地，所述位姿观测残差中e关于位姿

优选地，所述惯性预积分残差公式为：

其中，

本发明公开了以下技术效果：在相机机动较大时，仅仅依靠视觉特征进行运动估计，导航定位精确度低。本发明将语义特征以及惯性信息融合到视觉里程计的优化目标函数中，为视觉里程计增添高维的特征观测以及额外的传感器观测约束，对短时间内的相对运动进行估计，并进行位姿优化，可以提升其在机动性较强情况下的位姿估计精度，因此可以提升视觉里程计导航定位的精度与鲁棒性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

如图1所示，本发明提供一种融合语义特征的视觉惯性组合导航方法，包括如下步骤：

步骤1：采集k时刻RGBD视觉传感器数据S(k)、加速度计数据

步骤2：根据视觉传感器数据S(k)利用视觉里程计解算得到相机当前位姿T(k)：

首先对相邻两帧进行ORB特征提取，然后利用两帧之间的ORB特征匹配关系以及Perspective-n-point(pnp)解算出两关键帧之间的相对位姿，通过相对位姿的累加即可得到k时刻相机的位姿T(k)。

步骤3：基于视觉传感器数据S(k)进行相邻两图像帧之间的语义平面特征提取和匹配：

利用k时刻和k-1时刻的RGBD视觉传感器采集数据S(k)和S(k-1)，其对应的语义平面特征提取和匹配方法如下：

(a)提取语义信息

利用YOLOv3目标检测算法，实现设定语义对象的检测，获得语义对象在图像帧的二维框边界bbox。

(b)语义平面提取

在所述bbox区域内，将视觉传感器数据S(k)中的深度图像转化为结构化点云的格式，根据基于连通域的结构化点云平面分割算法，先利用邻域协方差进行点云的法向量提取，然后基于点云法向量以及其切平面与原点的距离，判断相邻点云是否属于同一连通域。

利用最小二乘法，对点云个数大于设定阈值N

(c)基于提取到的平面的信息，进行语义平面特征构建

首先利用平面法向n

然后，利用平面的语义类别、形心、法向以及水平垂直类型，构建语义平面特征：

·s

·s

·

·s

其中，s

(d)基于所述语义平面特征，构建语义平面特征图，将当前帧的语义平面与地图中的语义路标进行匹配。

基于所述步骤(b)得到的语义平面：

其中，S

将第一个接收到的语义平面直接映射为第一个语义路标，表示为L

其中

其中x

完成初始语义路标设置后，设计通过以下三个步骤实现每帧检测语义平面S

首先，将从帧中提取的语义平面与语义路标进行类别及平面类型的匹配，匹配成功后，计算语义平面的点云个数及区域面积，删除点云个数小于最小阈值t

利用坐标变换，将语义平面的法线方向由相机坐标系转换到世界坐标系下，得到

将语义平面的形心由相机坐标系转换到世界坐标系下，计算语义平面形心与其相匹配的路标形心的马氏距离，若其大于设定的阈值，则将检测到的语义对象映射为新的语义路标l

由上述步骤，可以得到当前关键帧与语义路标间的观测关系。

步骤4：基于惯性传感器数据进行相邻两图像帧之间的预积分：

所述k时刻得到的惯性传感器数据

其中，n

相邻两惯性帧之间的预积分过程：

其中，

步骤5：联合语义平面观测残差、视觉里程计相对位姿观测残差和惯性预积分残差优化求解载体导航信息：

a)建立优化变量X：

其中，

(b)建立优化函数e：

其中，Semantic部分代表语义路标观测的残差，

对于语义路标观测误差：

其中，

其中，ξ为位姿

对于VO位姿观测残差：

其中，

对于惯性预积分残差，其由相邻两图像帧之间的预测值与预积分值之差得到：

上式中，

(c)使用列文伯格-马夸特算法对优化函数进行迭代求解，当达到误差收敛或者设定的最大迭代次数时停止迭代，求得载体导航信息。

步骤6：输出载体导航信息和相机内参，并返回步骤1。

在相机机动较大时，仅仅依靠视觉特征进行运动估计，导航定位精确度低。本发明将语义特征以及惯性信息融合到视觉里程计的优化目标函数中，为视觉里程计增添高维的特征观测以及额外的传感器观测约束，对短时间内的相对运动进行估计，并进行位姿优化，可以提升其在机动性较强情况下的位姿估计精度，因此可以提升视觉里程计导航定位的精度与鲁棒性。

以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。