一种紧耦合视觉惯导系统参数的初始化方法

本发明涉及紧耦合视觉惯导系统参数的初始化方法，具体涉及一种相机与imu数据融合过程中参数的初始化。

背景技术：

1、三维扫描仪在实时扫描过程中，通常使用即时定位与地图构建(simultaneouslocalization and mapping，slam)技术实现三维扫描仪的位姿跟踪。但是，视觉slam存在输出频率低、旋转运动或运动速率加快时定位易失败等问题，而惯性测量单元(inertialmeasurement unit，imu)具有输出频率高、能输出6自由度测量信息等优点，因此现阶段的一个研究热点是将视觉slam与imu得到的位姿估计结果进行融合，得到更加准确的输出结果，而imu和视觉信息融合的前提是imu参数的初始化。

2、目前，通常采用kalibr标定工具箱、vins-mono算法和orb_slam3等开源算法实现视觉惯导系统的imu初始化，但是，kalibr标定工具箱配置麻烦，导致操作很麻烦，vins-mono和orb_slam3等方法初始化时间长、效率低、精度不高。上述初始化方式，由于操作复杂，效率低、费事，均难以应用到三维扫描仪上。

技术实现思路

1、本发明的目的在于克服现有技术的不足，提供一种能够实现高精度、易操作的紧耦合视觉惯导系统参数的初始化方法。

2、本发明的一种相机与imu视觉惯导系统初始化方法，包括如下步骤：

3、s110、为对带imu和相机传感器的视觉惯导系统中的imu和相机传感器做时间戳同步；

4、s120、对带imu和相机的视觉惯导系统进行初始化，然后使用视觉惯导系统拍摄带四个以上反光标志点物体的图像，获取图像过程中，保持视觉惯导系统做旋转、平移运动，通过imu测量得到物体的图像以及与物体的图像对应的加速度、角速度矢量惯性测量参数，某一帧图像对应的惯性测量参数为：若当前图像的前一帧图像在tn-1时刻采集完成，所述的当前图像在tn时刻采集完成，则tn-1时刻至tn时刻之间，imu测量得到的惯性测量参数均为所述的当前图像对应的惯性测量参数；

5、物体的图像通过相机拍摄得到，imu包括陀螺仪和加速度计，陀螺仪测量角速度矢量信息，加速度计测量加速度矢量信息；

6、s130、基于提取标志点的算法获取各帧图像中的反光标志点的信息，然后采用帧到模型的方法处理反光标志点的信息获取反光标志点的位姿，即各帧图像的位姿，各帧图像的位姿信息中包含当前时刻图像相对于前一时刻图像的旋转矩阵以及平移矩阵；然后利用前后两帧图像之间的平移矩阵除以采样时间得当前时刻图像对应的速度；当前时刻图像的旋转矩阵、平移矩阵和速度分别记为rj、tj、vj；

7、s140、对通过视觉惯导系统测得的与物体的图像对应的加速度、角速度矢量信息的测量值通过数学模型修正，得到优化后的加速度、角速度矢量信息；

8、其中，角速度优化数学模型公式如下：

9、ωm＝sω+bg

10、公式中，ωm为imu测量得到的角速度，s为比例因子，表示角速度测量值和真实值的比例误差，bg为陀螺仪偏置值，即陀螺仪的测量值和真实值之间的误差值，ω为优化后的角速度，视为角速度真实值；

11、加速度优化数学模型公式如下：

12、am＝sa+ba

13、其中，am为imu测量得到的加速度，s为比例因子，和角速度模型中取值一致；bg为加速度偏置值，即加速度的测量值和真实值之间的误差值，a为优化后的加速度，视为加速度真实值；

14、视觉惯导系统未初始化之前，imu参数的初始值ba、bg均为0，s为1；

15、s150、对角速度优化数学模型以及加速度优化数学模型中的各个参数的初始值进行求解，具体过程如下：

16、所述陀螺仪偏置bg的初始值的求解过程如下：

17、第一步，利用imu测量的角速度积分得到j时刻图像与i时刻图像之间的旋转矩阵

18、刻图像角速度测量值；i为前一帧图像的采集时刻，j为当前帧图像的采集时刻，δt为预设采集图像的时间间隔；为i时刻图像的旋转矩阵，表示把各个时刻的imu坐标系对齐到i时刻imu坐标系下；表示把t时刻的imu坐标系转换到i时刻的imu坐标系；

19、第二步，将通过提取标志点的算法得到的图像旋转矩阵rj与做差，展开得到：

20、

21、第三步，对第二步式中的bg求偏导；

22、第四步，用最小二乘优化方法对bg优化，即可得到陀螺仪偏置bg初始值；

23、所述重力方向gw、尺度因子s的初始值求解过程如下：

24、第一步，对利用imu测量得到的图像的加速度、角速度积分得到j时刻图像相对于前一时刻图像的平移矩阵旋转矩阵对应的数学公式如下所示：

25、

26、

27、

28、

29、其中，i为前一帧图像的采集时刻，j为当前帧图像的采集时刻，为i时刻图像相对于前一帧图像的平移矩阵，gw为当前图像坐标系下的重力方向，为i时刻图像相对于前一帧图像的旋转矩阵，表示把各个时刻的imu坐标系对齐到i时刻imu坐标系下，表示把t时刻的imu坐标系转换到i时刻的imu坐标系，为与当前帧图像对应的真实加速度，vtw为与当前帧图像对应的速度，为与t时刻图像对应的真实加速度，δt为预设的图像采样时间间隔；

30、第二步，分别将通过提取标志点的算法得到的j时刻图像的旋转矩阵rj与做差，j时刻图像的平移矩阵tj与做差，展开的数学公式如下：

31、

32、

33、第三步，分别对公式中的gw、s求偏导；

34、第四步，用最小二乘优化方法对gw、s优化，即可得到陀螺仪偏置gw、s初始值；

35、所述加速度偏置ba初始值的求解过程如下：

36、第一步，对利用imu测量得到的i时刻图像的加速度、角速度分别积分得到j时刻图像的平移矩阵速度vwj、旋转矩阵数学公式如下所示：

37、

38、

39、

40、其中vwj为与j时刻图像对应的图像速度，vwi为与i时刻图像对应的速度；

41、第二步，将通过提取标志点的算法得到的j时刻的旋转矩阵、平移矩阵分别与vwj做差，展开的数学公式如下：

42、

43、

44、

45、第三步，对上式中的ba、gw、bg分别求偏导；

46、第四步，用最小二乘优化方法对gw、ba、bg多个变量联合优化，即可得到加速度偏置ba初始值。

47、本发明的有益之处在于：

48、本方法能够获取带标志点图像、以及图像对应的加速度、角速度矢量信息，能够利用标志点跟踪的图像位姿作为真值优化图像对应的惯性测量参数即加速度、角速度矢量信息，得到精度较高的惯性测量参数，用于视觉惯导系统的初始化，以便于视觉惯导设备进行下一步的工作。并且采用本方法无需像kalibr标定工具箱一样要预先进行工具箱配置，操作简单，同时相比orb_slam3、vins_mono等方法更加高效省时。能够实现高精度、易操作的相机与imu系统的imu参数初始化。