一种解耦合的惯性/视觉导航在线动态标定装置和方法与流程

本发明属于属于无人机惯性/视觉导航，涉及一种解耦合的惯性/视觉导航在线动态标定装置和方法。

背景技术：

1、视觉导航在纹理丰富的静态区域往往能实现良好的跟踪效果，达到较高的定位精度，但是在弱纹理、光照剧烈变化、快速移动场景中容易定位失败。多传感器融合导航一直是导航领域的研究热点，惯性传感器不受运动状态和环境的影响，能够在视觉跟踪丢失状态下稳定的提供运动观测信息，而视觉传感器又能针对惯性传感器长时间发散的短板进行抑制，形成优势互补。惯性/视觉组合导航，是现在无人系统实际应用的主流导航方式之一。

2、在无人机导航领域，为了实现高精度的组合导航，减少器件误差，要求传感器间实现高精度的时空标定。现有的惯性/视觉传感器器标定多采用离线方式，在线标定方法较少且需要较大的运动激励，无人机常规空中飞行过程中很难做到三方向三角度的大动作激励，这对应用场景提出了较大的使用约束。

技术实现思路

1、本发明的目的在于克服现有技术中的不足，提供一种解耦合的惯性/视觉导航在线动态标定装置和方法。本发明方案能够解决上述现有技术中存在的问题。

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

3、根据第一方面，提供一种解耦合的惯性/视觉导航在线动态标定装置，包括惯性传感器、视觉传感器、旋转外参数计算单元和平移外参数计算单元，所述的惯性传感器获取目标的位姿信息，传送给所述的旋转外参数计算单元；所述的视觉传感器获取目标的视觉帧位姿信息，传送给所述的旋转外参数计算单元和平移外参数计算单元，所述的旋转外参数计算单元根据获取的惯性位姿信息和视觉帧位姿信息，计算视觉/惯性的旋转外参数和陀螺零偏，并校正陀螺零偏对惯性传感器积分获得的位姿信息的影响，完成惯性传感器的在线标定，将旋转外参数和校正后的位姿信息发送给所述的平移外参数计算单元，所述的平移外参数计算单元根据旋转外参数、惯性位姿信息、视觉帧位姿信息、相对速度信息获取位置约束方程，并对平移外参数进行初步估计后，将获得的平移外估计参数中对应的状态量作为先验信息，求解更精准的平移外参数和尺度因子，并利用获得的平移外参数和尺度因子对视觉传感器的视觉帧位姿信息进行更新，完成惯性/视觉传感器在线标定。

4、进一步的，所述的旋转外参数计算单元包括旋转外参数粗标定单元和旋转外参数精标定单元，所述的旋转外参数粗标定单元根据对极几何获取视觉传感器的视觉帧位姿信息和惯性传感器的惯性位姿信息构建约束方程获取粗估计的旋转外参数，并对陀螺零偏进行粗估值后，对惯性位姿信息进行粗校正，将粗校正后的惯性位姿信息发送给所述的旋转外参数精标定单元，所述的旋转外参数精标定单元通过ba优化获取到的视觉帧位姿信息和粗校正后的惯性位姿信息构建约束方程，获取精确的旋转外参数和陀螺零偏，并利用精确的陀螺零偏对惯性位姿信息进行校正，获得精确的惯性位姿信息，将旋转外参数和惯性位姿信息发送给所述的平移外参数计算单元。

5、进一步的，所述的平移外参数计算单元包括无绝对速度平移外参数估计单元和含绝对速度平移外参数估计单元，所述的无绝对速度平移外参数估计单元根据相对速度、旋转外参数、惯性位姿信息和视觉帧位姿信息，计算无绝对速度的平移估计参数，并将无绝对速度的平移估计参数发送给所述的含绝对速度平移外参数估计单元，所述的含绝对速度平移外参数估计单元将无绝对速度的平移估计参数作为先验信息，对含绝对速度平移外参数进行求解，获得尺度因子s和平移外参数，并利用尺度因子更新视觉帧位姿信息，完成惯性/视觉传感器的在线标定。

6、根据第二方面，提供上述一种解耦合的惯性/视觉导航在线动态标定方法，包括以下步骤：

7、获取视觉帧位姿信息和惯性位姿信息；

8、根据视觉帧位姿信息和惯性位姿信息建立同一时空的旋转约束误差方程；

9、采用基于双重优化的旋转约束求解方法，获取两个传感器的旋转外参数和陀螺零偏；

10、获得的陀螺零偏对惯性传感器进行校正，完成惯性传感器的在线动态标定；

11、根据视觉帧位姿信息、惯性位姿信息和旋转外参数，基于解耦合的平移外参估计方法，获取两个传感器的平移外参数，利用尺度因子更新视觉帧位姿信息，完成惯性/视觉传感器在线动态标定。

12、进一步的，所述的旋转约束误差方程为：

13、

14、其中，eq(zq,χq)表示旋转约束误差，χq为旋转约束参数，为滑窗内的陀螺常值零偏，ji是预积分旋转误差关于i时刻的陀螺零偏误差的一阶雅可比矩阵，且在优化前通常将陀螺零偏误差的初值设置为零，为i时刻到j时刻之间惯性传感器计算获取的惯性旋转量，为i时刻到j时刻之间视觉传感器求解的视觉旋转量，为视觉传感器与惯性传感器之间的旋转外参，为的逆矩阵。

15、进一步的，所述的双重优化的旋转约束求解方法包括以下步骤：

16、根据对极几何获取的视觉传感器的视觉帧位姿信息和惯性传感器的惯性位姿信息，采用旋转约束误差方程获取粗略的旋转外参数和陀螺零偏；

17、采用获取的粗略陀螺零偏对惯性位姿信息进行粗校正，获得粗校正惯性位姿；

18、将粗校正惯性位姿和根据ba优化后获取到的视觉帧位姿信息，采用旋转约束误差方程获取精确的旋转外参数和陀螺零偏。

19、进一步的，所述的基于解耦合的平移外参估计方法包括以下步骤：

20、通过相邻关键帧之间的视觉帧位姿信息、惯性位姿信息中已知的相对速度信息，获取位置约束方程：

21、其中表示t+1时刻和t时刻视觉传感器位置误差，和分别为t+1时刻和t时刻视觉传感器相对于c0系的估计值，分别为t、t+1时刻惯性传感器相对于c0系的估计值，为的逆矩阵，为求解中第t个滑动窗口对应的速度信息，为参考视觉传感器坐标系下的重力向量，为位置误差项，s为尺度信息，δt为时间间隔，为平移外参数。

22、在连续三帧的数据里，获取相邻帧之间的关系，在规避了绝对速度信息的情况下对χp参数进行估计：

23、

24、其中，χp为无绝对速度的平移估计参数；

25、通过在初始化前期所有的配对的视觉传感器与惯性传感器的位置计算代价函数；

26、通过对代价函数的求解获取χp结果；

27、通过滑窗内相邻关键帧之间的惯性传感器位置信息与速度信息建立残差方程：

28、

29、

30、其中，和代表位置和速度误差项；

31、将获取χp结果作为为先验信息，带入到上一步的残差方程中，对滑窗内的速度信息、尺度因子和平移外参数进行优化求解；

32、根据获得的速度信息，对含绝对速度的平移估计参数进行求解；其中s为惯性传感器量测位移与视觉传感器恢复位移的尺度因子，其中，为第n个滑窗中对应的速度信息，为参考视觉传感器坐标系下的重力向量。

33、根据第三方面，提供一种惯性/视觉导航装置，所述的导航装置安装有本发明所述的一种解耦合的惯性/视觉导航在线动态标定装置。

34、根据第四方面，提供一种无人机，所述的无人机安装有本发明所述的惯性/视觉导航装置。

35、根据第五方面，提供一种介质，所述的介质存储本发明所述的一种解耦合的惯性/视觉导航在线动态标定方法。

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

37、(1)本发明将旋转和平移外参数解耦，减少了在线标定的运动条件限制，增加了在线标定的应用场景，使在线标定可以使用在无人机等不适宜进行剧烈运动的场景；

38、(2)本发明通过旋转外参数和平移外参数解耦后，通过旋转外参数的求解，加速平移外参数的的收敛，加速了整个传感器初始化的迭代求解时间；

39、(3)本发明针对旋转外参标定精度低的问题，提出了基于双重优化的旋转约束求解方法，通过两轮传感器迭代优化，提高了外参估计的精度，进一步提升了惯性/视觉组合导航的定位精度。