[发明专利]一种融合语义特征的视觉惯性组合导航方法

说明书

本发明公开了一种融合语义特征的视觉惯性组合导航方法，采集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_p＝{p_x,p_y,p_z}

·s_n＝{n_x,n_y,n_z,n_d}

·

·s_c＝对应检测到的语义对象类别

其中，s_p.为平面形心，s_n为平面法线参数，s_o为平面类别标签(水平/垂直)，s_c.为平面语义类别标签，其取决于平面所对应的语义对象类别；

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

定义初始语义路标，将每帧检测语义平面S_k分别与所述初始语义路标的类别、法向、形心进行匹配判断，即得到当前关键帧与语义路标间的观测关系。