[发明专利]单目视觉信息和IMU信息相融合的尺度估计系统及方法

权利要求书

1.一种单目视觉信息和IMU信息相融合的尺度估计方法，包括：

步骤S1：通过单目相机采集视觉图像数据，通过对无绝对尺度信息的视觉图像数据进行处理，获得单目相机位姿数据，通过IMU模块获得IMU位姿数据；

步骤S2：基于角速度将IMU位姿数据和单目相机位姿数据进行时间空间对齐；以及

步骤S3：基于加速度在频域下进行尺度估计，完成基于单目视觉信息和IMU信息相融合的尺度估计；

步骤S2中，首先假设视觉角速度和IMU角速度在时间上是对齐的，通过采用最小二乘法最小化不含时间偏置的误差代价函数得到单目相机角速度和IMU角速度之间最优的空间变换关系：

其中，||C_ω||²指误差值，R_S表示从相机坐标系到IMU体坐标系的旋转矩阵，表示陀螺仪的偏置，为单目相机角速度，为IMU角速度；

因为视觉角速度和IMU角速度满足两组矢量的空间旋转和平移关系，利用如下空间对齐初值估计封闭解算法，首先定义两组量的角速度质心：

其中，分别为视觉角速度质心和IMU角速度质心，n是参与计算的数据个数，从而可以得到两组去质心的角速度：

其中，P_V，Q_I分别为去质心视觉角速度集和去质心IMU角速度集；那么定义角速度累积矩阵W：

对W进行奇异值分解：

W＝U∑V^T；

其中，∑为奇异值diag(σ₁，σ₂，σ₃)组成的对角矩阵，对角线元素依次从大到小排列，U和V为分解出的正交矩阵；从而，空间对齐的旋转矩阵R_S即为：

R_S＝V×C×U^T；

其中，W为满秩矩阵时，C＝diag(1，1，1)；W为秩亏矩阵时，C＝diag(1，1，-1)；在完成旋转矩阵的解算之后，IMU模块中陀螺仪的角速度的偏置根据下式求得：

利用基于黄金分割搜索的时间偏置和角速度偏置的迭代算法，先确定正负两个方向的最大时间偏置变化±(t_d)_max，之后对时间偏置使用黄金分割搜索，同时以空间对齐初值估计封闭解算法求解R_S和之后时间偏置和空间旋转的求解以迭代方式进行，具体如下，首先设黄金分割率为：

时间偏置的初值为：

a＝-(t_d)_max，b＝(t_d)_max；

两个时间偏置的黄金分割试探点分别为：

α＝b-(b-α)/ρ_g，β＝a+(b-α)/ρ_g；

当两个试探点的误差|α-β|小于阈值τ_tolerance时，则其均值为最终的时间偏置值：

t_d＝(α+β)/2；

得到R_S的最优解后，再将时间偏置项t_d包含进目标函数中，求解时间偏置的最优化结果：

之后空间对齐和时间对齐优化交替进行。