一种多传感器融合定位系统的位姿优化方法和装置与流程

技术特征：
1.一种多传感器融合定位系统的位姿优化方法，包括：将获取到的预设时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合，得到相对世界坐标系下的第一位姿；基于所述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到所述第二位姿的优化值和旋转外参的优化值，所述旋转外参的优化值用于下一时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合。2.如权利要求1所述的方法，所述对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到所述第二位姿的优化值和旋转外参的优化值，具体包括：迭代优化所述第二位姿中的第二位置和第二航向角及所述旋转外参，得到所述第二位置的优化值、第二航向角的优化值和旋转外参的优化值。3.如权利要求1所述的方法，所述基于所述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，具体包括：根据所述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，建立下述至少一项残差，将建立的各残差组合成目标残差：所述第一位姿中的第一位置与所述第一位姿转换到绝对世界坐标系下的第二位姿中的第二位置的相对位置变化残差；所述第一位姿中的第一姿态与所述第一位姿转换到绝对世界坐标系下的第二位姿中的第二姿态的相对姿态变化残差；所述第一姿态的绝对姿态转换残差；全球定位系统在绝对世界坐标系下的观测位置的绝对位置残差；全球定位系统在绝对世界坐标系下的观测航向的绝对姿态残差。4.如权利要求3所述的方法，所述相对位置变化残差，通过下述步骤建立：将相邻两帧图像拍摄时刻的第一位置的差值，根据所述两帧中的前一帧图像拍摄时刻惯导坐标系与相对世界坐标系的旋转外参，转换成所述前一帧图像拍摄时刻惯导坐标系下的第一差值；根据所述两帧图像拍摄时刻的待优化的第二位姿中的第二位置的差值与所述前一帧图像拍摄时刻第二位姿中的第二姿态，确定所述前一帧图像拍摄时刻惯导坐标系下的第二差值；将所述第一差值与所述第二差值间的差值确定为相对位置变化残差。5.如权利要求3所述的方法，所述相对姿态变化残差，通过下述步骤建立：根据相邻两帧图像拍摄时刻的待优化的第二位姿中的第二姿态，和分别在所述两帧图像拍摄时刻惯导坐标系与相对世界坐标系的旋转外参，确定相对姿态变化残差。6.如权利要求3所述的方法，所述第一姿态的绝对姿态变化残差，通过下述步骤建立：根据惯导坐标系与相对世界坐标系的旋转外参、待优化的第二位姿中的第二姿态和相对世界坐标系与绝对世界坐标系间的旋转外参，确定所述第一姿态的绝对姿态变化残差。7.如权利要求3所述的方法，全球定位系统在绝对世界坐标系下的观测位置的绝对位
置残差，通过下述步骤建立：根据全球定位系统在绝对世界坐标系下的观测位置、待优化的第二位姿中的第二位置和第二姿态，及惯导坐标系与绝对世界坐标系间的旋转外参，确定全球定位系统在绝对世界坐标系下的观测位置的绝对位置残差。8.如权利要求3所述的方法，全球定位系统在绝对世界坐标系下的观测航向的绝对姿态残差，通过下述步骤建立：根据全球定位系统在绝对世界坐标系下的观测航向和待优化的第二位姿中的第二姿态，确定全球定位系统在绝对世界坐标系下的观测航向的绝对姿态残差。9.如权利要求1～8任一所述的方法，所述基于所述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化前，还包括：判断视觉传感器拍摄的图像数据与惯导测量数据融合的次数是否达到预设次数；或，判断当前获取到的第一位姿对应的帧数是否达到预设帧数。10.一种多传感器融合定位系统的位姿优化装置，包括：融合模块，用于将获取到的预设时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合，得到相对世界坐标系下的第一位姿；优化模块，用于根据所述融合模块得到的第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到所述第二位姿的优化值和旋转外参的优化值，所述旋转外参的优化值用于下一时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合。11.一种计算机可读存储介质，其上存储有计算机指令，当该指令被处理器执行时实现权利要求1～9任一所述的多传感器融合定位系统的位姿优化方法。

技术总结
本发明公开了一种多传感器融合定位系统的位姿优化方法和装置。所述方法包括：将获取到的预设时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合，得到相对世界坐标系下的第一位姿；基于述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到第二位姿的优化值和旋转外参的优化值，旋转外参的优化值用于下一时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合。能够快速准确的对多传感器融合定位系统中的位姿进行优化。系统中的位姿进行优化。系统中的位姿进行优化。


技术研发人员：韩冰 张涛 边威 黄帅
受保护的技术使用者：阿里巴巴集团控股有限公司
技术研发日：2020.09.30
技术公布日：2022/4/12