一种基于短期惯性测量的视觉连续定位方法与流程

本发明属于无人机视觉导航，具体涉及一种基于短期惯性测量的视觉连续定位方法。

背景技术：

1、随着视觉传感器技术、计算机技术和人工智能技术的迅速发展，计算机视觉领域中目标识别、视觉跟踪、视觉建图与视觉定位等研究方向都有一系列技术突破，视觉导航开始在自主导航领域占据一席之地。视觉传感器具有隐蔽性、轻便、低功耗和价格低廉等特点，因此利用视觉传感器进行导航具有很大的优势。

2、视觉导航在纹理丰富区域往往能实现良好的跟踪效果，达到较高的定位精度。但是视觉在光照剧烈变化、特征稀疏和快速移动场景极易跟踪丢失，每次跟踪丢失后视觉传感器将重新初始化。由于视觉定位为局部坐标系(非地理系)，每次初始化视觉坐标系具有随机性，用户使用过程中将得到一段段分离的定位结果，难以满足用户实际使用中定位连贯有效的需求。

技术实现思路

1、本发明的目的在于克服现有技术中的不足，提供一种基于短期惯性测量的视觉连续定位方法。本发明方案能够解决上述现有技术中存在的问题。

2、本发明的技术解决方案：

3、根据第一方面，提供一种基于短期惯性测量的视觉连续定位方法，包括以下步骤：

4、步骤一，视觉传感器初始化完成时刻t0后，通过视觉跟踪对载体进行定位，获得视觉位姿信息；

5、步骤二，t1时刻视觉跟踪丢失，使用融合惯性测量的视觉重定位方法进行重定位，若重定位成功，则继续步骤一进行视觉跟踪定位；若重定位不成功，重新进行视觉传感器初始化，初始化在t2时刻完成，进行步骤三；

6、步骤三，惯性传感器在t1和t2时间间隔内进行惯性测量，获取载体的惯性姿态变化信息；

7、步骤四，利用惯性姿态变化信息，将t2时刻初始化构建的世界坐标系对齐至t0时刻初始化构建的世界坐标系，获得视觉连贯鲁棒的定位结果。

8、进一步的，所述的融合惯性测量的视觉重定位方法，包括以下步骤：

9、使用惯性测量对载体运动进行预估，获得下一周期的惯性位姿，并发送给视觉传感器；

10、所述的视觉传感器根据获得的惯性位姿，转移到视觉相机坐标系下，划定视觉特征匹配区域，实现视觉匹配初定位；

11、在视觉特征匹配区域中，将当前帧与上一个视频帧、上一个参考帧以及上一个关键帧进行匹配，得到三组不同的特征匹配对；

12、利用对极几何约束八点法可得到当前帧到上一个视频帧的位姿变化矩阵包括旋转和平移，当前帧到上一个参考帧的位姿变化矩阵当前帧到上一个关键帧的位姿变化矩阵第一轮通过加权融合获取当前帧的视觉位姿信息：

13、

14、其中位姿矩阵代表第i帧(当前帧)对应的世界坐标系向相机坐标系的转换，分别代表上一视频帧、上一个参考帧和上一个关键帧对应的世界坐标系向相机坐标系的转换；w1、w2和w3分别为上一视频帧、上一个参考帧和上一个关键帧对应视觉位姿权重，考虑到其影响不同，应动态满足下列关系：

15、

16、

17、

18、当n1＜10时,n1取值为0；当n2＜10时,n2取值为0；

19、当n3＜10时,n3取值为0；

20、其中n1、n2、n3分别代表上一视频帧、上一个参考帧和上一个关键帧对应匹配点对数量，匹配数量越多，相机位姿求解的置信度越高；当匹配对数量低于10个时，该相机位姿结果为不可信，权重取0，由此得到第一轮重定位的相机位姿估计值

21、第二轮优化模型利用的是局部地图点与当前帧的3d-2d特征匹配，构造视觉重投影误差，最小化重投影误差可以表示为下式：

22、

23、其中m表示地图点id，xm为世界坐标系下三维地图点坐标，pam表示第a帧上xm对应的像素观测点，深度表示为π；

24、用第一轮求得的相机位姿估计值作为初值，带入上述迭代求解的过程中，最后获取视觉重定位的精准相机位姿估计值

25、进一步的，所述的惯性传感器在t1和t2时间间隔内进行惯性测量，获取载体的惯性姿态变化信息的方法为：

26、计算从i时刻到j时刻的惯性位姿变化，包括旋转rwbj和平移pj：

27、

28、

29、

30、其中，和分别表示k时刻陀螺仪和加速度计的偏差，由粗标定获取，和分别表示k时刻陀螺仪和加速度计的测量值，rwbi表示i时刻从惯性载体系坐标系到世界坐标系w1的旋转，rwbj表示j时刻从惯性载体系坐标系到世界坐标系w1的旋转，rwbk表示k时刻从惯性载体系坐标系到世界坐标系w1的旋转，g表示世界坐标系w1下的重力加速度，vi、vk和vj分别表示i时刻、k时刻和j时刻世界坐标系w1下的速度，pi和pj分别表示i时刻和j时刻世界坐标系w1下的位置，δt为积分时间间隔，i的取值为t1，j的取值为t2。

31、进一步的，所述的将t2时刻初始化构建的世界坐标系对齐至t0时刻初始化构建的世界坐标系下的步骤为：

32、视觉跟踪过程中，将求解到每一时刻相机系到世界坐标系w1的姿态转换矩阵，记录下t1时刻相机坐标系c2到世界坐标系w1的姿态转换矩阵，表示为tw1c2；

33、利用外参矩阵，将相机系转换到惯性载体系中，如下式所示：

34、tw1b1＝tw1c2tcb

35、其中tcb为外参矩阵，表示从载体系到相机系的位姿变化，tw1b1为t1时刻惯性载体系b1到世界坐标系w1的姿态转换矩阵，包括旋转和平移；

36、通过惯性导航获取t1时刻到t2时刻的载体运动，求解得到t2时刻惯性载体系b2到世界坐标系w1的姿态转换矩阵，表示为tw1b2；

37、利用外参矩阵，将惯性载体系转换到相机系中，如下式所示：

38、tw1c3＝tw1b2tcb-1

39、其中tw1c3为t2时刻相机坐标系c3到世界坐标系w1的姿态转换矩阵，包括旋转和平移；

40、利用t2时刻的相机系和世界坐标系的转换关系，完成最后的坐标系变换，见下式：

41、tw1w2＝tw1c3tw2c3-1＝tw1c3tc3w2

42、其中tw1w2为t2时刻世界坐标系w2到为t0时刻世界坐标系w1的姿态转换矩阵，包括旋转和平移；tw2c3表示从相机系坐标系c3到世界坐标系w2的位姿转换矩阵，包括3*3的旋转矩阵和3*1的平移向量。由于坐标系重合，这里旋转矩阵为单位阵，平移向量为0。

43、至此，得到两个世界坐标系的转换，在视觉跟踪丢失后，重新构造的位姿轨迹可以从世界坐标系w2转换到世界坐标系w1中，从而得到视觉连续有效的定位结果。

44、根据第二方面，提供一种介质，所述介质存储本发明所述的一种基于短期惯性测量的视觉连续定位方法。

45、根据第三方面，提供一种导航控制系统，所述的导航控制系统使用本发明所述的一种基于短期惯性测量的视觉连续定位方法进行导航的定位控制。

46、本发明与现有技术相比的有益效果：

47、(1)本发明提出一种融合惯性测量的视觉重定位方法，利用惯性测量辅助视觉匹配定位，同步提出一种加权匹配视觉重定位模型获取了更精准的视觉位姿信息，进一步提高了视觉重定位的成功率和精度。

48、(2)本发明针对视觉跟踪丢失后重新初始化、视觉定位不连贯的问题，研究一种视觉丢失状态下基于短期惯性测量的视觉初始化坐标转换方法，利用惯性传感器定位构造两组视觉局部坐标系的转换关系，实现连贯鲁棒的导航定位。