[发明专利]基于行程感知与视觉融合的定位方法、系统、终端和存储介质

权利要求书

1.一种基于行程感知与视觉融合的定位系统，其特征在于，包括：

视觉数据获取模块，所述视觉数据获取模块用于获取相机或摄像头数据，并根据相机或摄像头数据获取时间的时间戳分别给数据标签；

行程数据获取模块，所述行程数据获取模块包括航向角Y_ij获取模块、位移获取模块、旋转关系获取模块；

约束模块，所述约束模块用于以行程数据获取模块获得数据以约束关系约束视觉数据获取模块，对视觉数据获取模块计算结果进行优化。

2.根据权利要求1所述的基于行程感知与视觉融合的定位系统，其特征在于，还包括代价函数优化模块，所述代价函数约束模块，代价函数约束模块的影响因子，所述影响因子可以用于辅助判断代价函数的相对可信度。

3.根据权利要求2所述的基于行程感知与视觉融合的定位系统，其特征在于，所述航向角Y_ij获取模块获取每一个车轮的轮速脉冲编码器的读数、车轮轮胎的半径r数值、车轮轮胎每转一圈的脉冲计数ppr，以同一主动轴两车轮中第一车轮的轮速脉冲编码器乘以第一脉冲计数减第二车轮的轮速脉冲编码器乘以第二脉冲计数除以车轮轮胎直径d，获得航向角Y_ij。

4.根据权利要求1所述的基于行程感知与视觉融合的定位系统，其特征在于，所述位移获取模块获取每一个车轮的轮速脉冲编码器的读数、车轮轮胎的半径r数值、车轮轮胎每转一圈的脉冲计数ppr，以同一主动轴两车轮中第一车轮的轮速脉冲编码器乘以第一脉冲计数减第二车轮的轮速脉冲编码器乘以第二脉冲计数除以航向角Y_ij乘以罗德里格斯角获得位移。

5.根据权利要求4所述的基于行程感知与视觉融合的定位系统，其特征在于，所述约束模块包括第一约束模块和第二约束模块：所述第一约束模块以相同的自i至j时刻以车身里程计数据产生的运动增量约束相机运动增量，所述第一约束模块是约束车辆平移矢量的约束。

6.根据权利要求5所述的基于行程感知与视觉融合的定位系统，其特征在于，所述约束模块包括第一约束模块和第二约束模块：所述第二约束模块以轮速里程计的读数差获得车身航向角偏转来作为旋转增量的约束，所述第二约束模块是约束车辆旋转矢量的约束。

7.根据权利要求6所述的基于行程感知与视觉融合的定位系统，其特征在于，所述航向角Y_ij获取模块，获得的航向角Y_ij，位移获取模块和旋转关系获取模块分别为ΔL_ij和ΔR_ij，则：

根据公式(3)和公式(4)可以获得航向角Y_ij和两帧之间的位移增量d_ij；

其中，ΔL_ij表示后左轮第i时刻到第j时刻的增量，ΔR_ij表示后右轮第i时刻到第j时刻的增量，d_ij表示根据洛必达法则计算得出的两帧之间的位移增量、是一个脉冲读数对应的长度，所以S(ΔR_ij+ΔL_ij)/2为后左轮和后右轮路程的平均数，Y_ij表示航向角的角度。