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

权利要求书

1.一种智能体位姿估计器联合初始化方法，其特征在于，智能体位姿估计器包括集成在智能体上的相机、IMU和GNSS接收机，其联合初始化方法包括以下步骤：

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

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

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

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

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

S6、GNSS/IMU/视觉信息联合初始化：获取智能体GNSS轨迹，在地心地固坐标系下，将IMU/视觉联合轨迹与智能体GNSS轨迹进行对齐，完成智能体位姿估计器的联合初始化。

2.根据权利要求1所述的一种智能体位姿估计器联合初始化方法，其特征在于，步骤S1中IMU测量模型包括陀螺仪测量模型与加速度计测量模型，IMU原始测量数据包括陀螺仪偏置、陀螺仪噪声、地球重力加速度、加速度计偏置和加速度计噪声，陀螺仪测量模型测量陀螺仪偏置与陀螺仪噪声，加速度计测量模型测量地球重力加速度、加速度计偏置和加速度计噪声；

以机载相机的视频输出间隔作为积分时间差，在机器人坐标系下对IMU原始测量数据进行预积分，t_k时刻到t_k+1时刻IMU预积分项表示为：

所述公式中为IMU朝向的预积分项、为IMU速度的预积分项、为IMU位置的预积分项；为IMU朝向预积分项的测量值、为IMU速度预积分项的测量值、为IMU位置预积分项的测量值；和分别表示陀螺仪偏置和加速度计偏置的初始值，分别代表与雅可比矩阵相对应的子矩阵。当陀螺仪或加速度计偏置发生变化时，可以利用上述一阶雅可比近似来替代IMU预积分项，而无需重新积分。

3.根据权利要求1所述的一种智能体位姿估计器联合初始化方法，其特征在于，步骤S2中将角点特征分布约束为相邻特征之间的像素间隔大于等于预先设定的阈值，对后续图像帧的跟踪采用KLT稀疏光流跟踪；

设置滑动窗的阈值D，利用PnP算法计算两个相邻图像帧的相对运动关系，并用BA算法优化滑动窗内所有角点特征的重投影误差，获得智能体的初始位姿，包括智能体的初始位置和初始朝向。

4.根据权利要求1所述的一种智能体位姿估计器联合初始化方法，其特征在于，步骤S2中跟踪的角点特征少于设定的阈值时，重新执行步骤S2对视觉信息进行预处理。