[发明专利]一种基于双目相机与惯导传感器信息融合的定位方法

权利要求书

1.一种基于双目相机与惯导传感器信息融合的定位方法，其特征在于，包括以下步骤：

步骤S1、采集机器人周边图像序列；

步骤S2、对获取的图像帧，使用改进后的FAST特征点提取算法，并引入四叉树快速筛选特征点，使得FAST特征点分均匀分布；

步骤S3、提取图像帧的线特征，计算线特征描述子；

步骤S4、对前后图像帧提取的FAST特征点进行跟踪匹配，获取帧间估计位姿，再由该估计位姿，将前后帧中的点、线特征重投影到当前帧，并对线特征进行跟踪匹配；所述对线特征进行跟踪匹配则通过最小化重投影误差实现：

其中l＝[l₁,l₂]^T是投影直线，l₁代表前帧中待匹配的线特征，l₂代表后帧中待匹配的线特征，p^T、q^T是匹配线段的两个端点的齐次坐标；

步骤S5、确认当前帧是否为关键帧，若非关键帧则丢弃当前帧，并把其视觉信息与IMU预积分信息暂存传递给下一帧；

步骤S6、将当前帧加入滑动窗口，结合滑窗内已配对的点线特征约束与IMU预积分紧耦合进行后端优化，输出帧间运动位姿。

2.根据权利要求1所述的一种基于双目相机与惯导传感器信息融合的定位方法，其特征在于：所述改进后的FAST特征点提取算法具体为：根据图像大小划分网格；遍历每个网格，在每个网格上提取FAST特征点，将图像视为一个N叉树根节点，即N等分，如果根节点特征点个数大于1，则把该节点迭代等分生成四个子节点，迭代终止条件为当某个子节点内只含一个特征点或不含特征点；最终选取的特征点即为最优。

3.根据权利要求2所述的一种基于双目相机与惯导传感器信息融合的定位方法，其特征在于：步骤S3具体是使用LSD线特征提取算法提取线特征，并使用LBD算法计算线特征描述子。

4.根据权利要求3所述的一种基于双目相机与惯导传感器信息融合的定位方法，其特征在于：所述LSD(Line Segment Detector)线特征提取算法具体为：通过高斯采样缩小图像尺寸为原图的a％，在使得图像不变模糊的前提下减弱原图中存在的锯齿效应，对图像中每一点的像素梯度进行排序，把像素梯度方向的像素点进行联合，生成直线支持区域。

5.根据权利要求4所述的一种基于双目相机与惯导传感器信息融合的定位方法，其特征在于：所述LBD(Line Band Descriptor)算法具体为：在检测出的LSD线特征进行像素梯度累加，通过累计线特征中各行相关联的梯度信息构造描述符，计算各行描述符矩阵的均值和标准差，基于最近邻匹配准则，根据描述符的距离对线端进行匹配；

LBD描述子具体为：确定LSD线段条带区域与梯度方向；计算条带每行梯度权重；统计每行梯度直方图，构成描述符矩阵；计算均值和标准差串联成LBD描述子。

6.根据权利要求5所述的一种基于双目相机与惯导传感器信息融合的定位方法，其特征在于：在步骤S4中，对前后帧特征点进行跟踪匹配通过基于最小化光度误差的直接法进行，选取特征点及其周围的像素块光度误差最小作为最终的误差函数；采取双线性插值法来求取其灰度值，最小化光度误差函数为：

E_u,v＝I_v[p`]-I_u[p]

其中p为前图像帧I_u中的FAST特征点，p`表示p在当前图像帧I_v中的反投影坐标。

7.根据权利要求6所述的一种基于双目相机与惯导传感器信息融合的定位方法，其特征在于：在步骤S5中，关键帧选取策略为：

已连续N次当前帧没有被视为关键帧，当前帧能够跟踪到不少于h个FAST点特征和至少m条线特征，或者当前帧与参考关键帧中的点特征和线特征共有数量不超过参考关键帧的3/4。