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

权利要求书

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

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

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

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

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

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

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

2.根据权利要求1所述的一种融合语义特征的视觉惯性组合导航方法，其特征在于，

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

3.根据权利要求2所述的一种融合语义特征的视觉惯性组合导航方法，其特征在于，

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

· 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分别与所述初始语义路标的类别、法向、形心进行匹配判断，即得到当前关键帧与语义路标间的观测关系。

4.根据权利要求1所述的一种融合语义特征的视觉惯性组合导航方法，其特征在于，所述k时刻得到的惯性传感器数据包括从k-1时刻到k时刻的加速度计数据和陀螺仪数据用于构建惯性传感器测量模型：

其中，n_a和n_ω分别为加速度计和陀螺仪白噪声；和为加速度计和陀螺仪的随机游走，其导数为白噪声；为加速度计量测的理想值，为陀螺仪量测的理想值；g^W为导航系下的重力；为采样时刻导航坐标系到机体坐标系的旋转矩阵；

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

其中，为位置预积分，为速度预积分，旋转用四元数γ表示，为旋转预积分；初始时，和为0，为单位四元数，R(γ)表示将四元数转化为旋转矩阵，表示四元数的乘法运算；

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

5.根据权利要求1所述的一种融合语义特征的视觉惯性组合导航方法，其特征在于，所述优化函数为：

其中，Semantic部分代表语义路标观测的残差，表示相机某帧观测到的语义路标观测误差，VO部分代表视觉里程计中相对位姿的观测残差，第三项为IMU预积分估计残差，ρ(·)代表鲁棒核函数，Σ^-1表示各误差量对应的信息矩阵，其为协方差的逆，表示对各约束精度的预先估计，i、j分别指第i、j图像帧，k指的是第k个语义路标。