[发明专利]一种智能体位姿估计器联合初始化方法

说明书

本发明公开了一种智能体位姿估计器联合初始化方法，其步骤包括：首先，构建智能体状态向量，所述状态向量包含智能体当前朝向、位置、速度、陀螺仪和加速度计偏置、GNSS接收机偏置和偏置率、视觉特征深度、里程计坐标系与世界坐标系的偏置信息，并对智能体机载传感器测量数据进行预处理；然后，利用PnP算法求解纯视觉条件下的智能体位姿，并将视觉与IMU的联合初始化构建为最小二乘优化问题，通过带入视觉与IMU先验信息即可完成视觉与IMU的联合初始化；最后，利用SPP算法求解智能体在地心地固坐标系下的粗略位置，并构建GNSS伪距、多普勒频移和接收机时钟偏置的概率因子图模型，即可获得智能体位姿估计器的所有初值。

技术领域

本发明涉及多传感器融合位姿估计器技术，特别是涉及一种智能体位姿估计器联合初始化方法。

背景技术

相比于基于单一传感器的智能体位姿估计器，多传感器融合位姿估计技术能够充分利用不同种类传感器之间的互补特性，以获得更为准确且鲁棒的智能体位姿估计结果。立体相机和惯性测量单元(IMU)能够为智能体提供机器人坐标系下厘米级精度的位姿，但局部坐标系下的位姿会随着智能体的运动而漂移。全球导航卫星系统(GNSS)为能够为智能体提供无漂移的位置信息，已广泛应用于各种移动机器人导航任务。但基于 GNSS的智能体导航方法难以在室内使用，且易受噪声和多径效应的影响，导致其只能达到米级的定位精度。基于GNSS/IMU/视觉信息紧耦合的智能体位姿估计算法能够充分利用导航卫星、加速度计、陀螺仪、机载相机各自的优势，以获取准确、无漂、平滑的智能体位姿估计。

尽管基于GNSS/IMU/视觉信息紧耦合的智能体位姿估计算法具有较高的性能优势，但是由于紧耦合算法的非线性性质，位姿估计器的性能严重依赖于系统初始值。误差较大的系统初值将会导致智能体位姿估计器发散，甚至完全错误的位姿估计结果。在基于局部位姿传感器与全局位姿传感器融合的导航系统中，由于局部坐标系与全局坐标系之间的变换矩阵是一个动态变化的变量，变换矩阵必需随着智能体的运动而实时修正。这导致了基于GNSS/IMU/视觉信息紧耦合的智能体位姿估计器必需采取在线初始化的方式进行联合初始化。因此，如何快速、准确、鲁棒地获取智能体位姿估计器的初值已成为智能机器人导航领域亟需解决的难题。

发明内容

发明目的：本发明的目的是提供一种智能体位姿估计器联合初始化方法，以提高位姿估计器初值估计精度，且能够实时修正系统初值。

技术方案：本发明的一种智能体位姿估计器联合初始化方法，智能体位姿估计器包括集成在智能体上的相机、IMU和GNSS接收机，其联合初始化方法包括以下步骤：

S1、对IMU原始测量数据进行预处理：构建机器人坐标系下的IMU测量模型，获得IMU原始测量数据，在机器人坐标系下对IMU原始测量数据进行预积分，并对IMU 预积分项进行修正；

S2、对视觉信息进行预处理：提取第一帧图像的Shi-Tomasi角点特征，并以此为基础对后续图像帧进行角点特征跟踪，获取角点特征的帧间相对关系，计算两个相邻图像帧的相对运动关系，并优化滑动窗内所有角点特征的重投影误差，从而获得智能体的初始位姿；

S3、视觉与IMU联合初始化：将步骤S2获得的每个图像帧对应的智能体初始位姿带入陀螺仪偏置代价函数求得陀螺仪偏置，构建智能体状态向量χ，将智能体观测模型构建为最小二乘问题，并将智能体状态向量χ带入智能体观测模型，求解机器人坐标系下滑动窗内每个图像帧当前的智能体朝向、速度、位置和重力加速度信息，得到机器人坐标系r与里程计坐标系o之间的旋转矩阵利用旋转矩阵将智能体状态向量χ中的速度和位置信息投影到里程计坐标系，获得IMU/视觉联合轨迹；

S4、利用GNSS原始信号中的伪距信息粗略求解智能体的位置：根据GNSS原始信号中的伪距信息构建导航卫星与GNSS接收机的距离模型，计算智能体在地心地固坐标系下的智能体经纬度，同时获取地心地固坐标系与世界坐标系之间的旋转矩阵

S5、对齐智能体里程计坐标系与世界坐标系，获得里程计坐标系与世界坐标系之间的变换矩阵