[发明专利]视觉惯导里程计、位姿时序偏差估计方法和定位方法

权利要求书

1.一种视觉惯导里程计位姿时序偏差估计方法，其中，所述方法包括：

从图像中提取视觉特征，并且建立视觉特征与高精地图的数据关联，所述图像是视觉里程计集成的相机采集的；

获取所述视觉惯导里程计的先验位姿；

基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，并且估计所述相机的位姿和视觉惯导里程计位姿时序偏差。

2.根据权利要求1所述的方法，所述基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，建立视觉惯导里程计位姿时序偏差的估计模型，包括：

基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，分别确定所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项。

3.根据权利要求2所述的方法，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：

根据所建立的视觉特征与高精地图的数据关联，以获取与视觉特征匹配的地图三维特征；

获取与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标；

基于所述时序位姿变量、所述地图三维特征和与所述地图三维特征匹配的视觉特征在所述图像上的像素坐标，计算所述视觉惯导里程计的视觉特征重投影误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述视觉特征对所述相机的位姿变量的约束。

4.根据权利要求2所述的方法，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：

基于指定时刻的相机的时序位姿变量和所述视觉惯导里程计的位姿偏差，利用预设函数计算指定时刻的第一预测值；

基于获取的指定时刻的所述视觉惯导里程计的先验位姿和所述指定时刻的第一预测值，计算所述先验位姿误差作为所述视觉惯导里程计位姿时序偏差的估计模型中对所述相机的位姿变量和所述视觉惯导里程计的位姿偏差的约束。

5.根据权利要求2所述的方法，基于所建立的所述视觉特征与高精地图的数据关联和所述视觉惯导里程计的先验位姿，确定所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中的约束项，包括：

基于预设的自回归位姿偏差估计模型，利用指定时刻之前的所述视觉惯导里程计的预设数量时刻的位姿偏差计算所述视觉惯导里程计的指定时刻的位姿偏差；

基于计算出的所述视觉惯导里程计的指定时刻的位姿偏差和指定时刻之前的预设数量时刻的时序偏差，计算所述视觉惯导里程计的位姿偏差误差作为所述视觉惯导里程计位姿时序偏差的估计模型中所述自回归位姿偏差估计模型的计算过程的约束。

6.根据权利要求2至5任一项所述的方法，所述估计所述相机的位姿和视觉惯导里程计位姿时序偏差，包括：

基于所述视觉惯导里程计的视觉特征重投影误差、先验位姿误差、位姿偏差误差，计算所述视觉惯导里程计总误差；

利用预设非线性优化算法，通过使得所述视觉惯导里程计位姿的总误差最小化，估计所述相机的位姿和所述视觉惯导里程计的位姿时序偏差。