[发明专利]视觉惯导方法、装置、设备及计算机可读存储介质

权利要求书

1.一种视觉惯导方法，其特征在于，所述方法包括：

通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值；

确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项；

根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计。

2.根据权利要求1所述的视觉惯导方法，其特征在于，所述通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值，包括：

在上一时刻到当前时刻的时间段内，对所述惯导传感数据中的加速度计数值以及角速度计数值进行累计，同时，结合所述视觉模组在上一时刻的状态估计，得到所述状态预测值。

3.根据权利要求1所述的视觉惯导方法，其特征在于，所述确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项，包括：

根据所述视觉模组获取的RGB图像或灰度图像进行特征提取和匹配，得到被跟踪的特征点及其观测值。

4.根据权利要求1所述的视觉惯导方法，其特征在于，所述确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项，包括：

结合所述多相机状态对所述特征点的观测值，计算所述特征点在世界坐标系中的坐标估计值，再将所述坐标估计值投影到所述多相机状态中，得到所述特征点的观测值估计值。

5.根据权利要求4所述的视觉惯导方法，其特征在于，所述结合所述多相机状态对所述特征点的观测值，计算所述特征点在世界坐标系中的坐标估计值，再将所述坐标估计值投影到所述多相机状态中，得到所述特征点的观测值估计值，包括：

根据所述观测值确定所述特征点在对应的深度图中的深度值，以及，确定所述深度值是否处于预设的深度阈值范围；

若所述深度值处于预设的深度阈值范围，则通过加权最小二乘法计算所述特征点在所述世界坐标系中的、或对于相应相机状态的坐标估计值，并将计算得到的坐标反投影到所述多相机状态中，得到所述多相机状态对所述特征点的第二观测值估计值；

若所述深度值未处于所述深度阈值范围，则通过三角化法计算所述特征点在所述世界坐标系中的、或对于相应相机状态的坐标估计值，并将计算得到的坐标反投影到所述多相机状态中，得到所述多相机状态对所述特征点的第一观测值估计值。

6.根据权利要求5所述的视觉惯导方法，其特征在于，所述所述确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项，还包括：

对所述第二观测值估计值和所述观测值进行差值计算，得到三维残差项；

以及，对所述第一观测值估计值和所述观测值进行差值计算，得到二维残差项。

7.根据权利要求6所述的视觉惯导方法，其特征在于，所述根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计，包括：

通过所述二维残差项对所述状态预测值进行第一次修正；

以及，通过所述三维残差项对所述第一次修正后的位姿估计进行第二次修正，得到当前时刻的位姿估计。

8.一种视觉惯导装置，其特征在于，所述装置包括：

状态预测模块，用于通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值；

残差构建模块，用于确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项；

位姿修正模块，用于根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计。