[发明专利]一种基于惯导复用的视觉、惯导、卫导融合定位方法

权利要求书

1.一种基于惯导复用的视觉、惯导、卫导融合定位方法，其特征在于，包括以下步骤：

步骤1，获取惯导零偏、双目相机的内参矩阵以及惯导、双目相机、卫星天线三种传感器之间的变换矩阵；

步骤2，获得惯导的三轴加速度和三轴角速度数据、双目视觉数据、，通过时间戳对惯导的加速度、角速度与双目视觉数据进行匹配，基于图优化的视觉SLAM框架对惯导和视觉数据进行融合，计算得到与双目视觉数据同频率的视觉-惯导一级定位结果；

步骤2.1，根据惯导的三轴加速度和三轴角速度数据计算第k帧和第k+1帧之间的积分结果，得到k+1帧的位置、速度和姿态；

步骤2.2，根据步骤2.1计算出的预积分信息，实现视觉、惯导相互约束，完成初始化；

步骤2.3，构建目标函数，目标函数包含边缘化先验信息、惯性测量残差和视觉重投影残差三部分，通过优化各帧状态向量，使目标函数达到最小值，解算出各帧最终的位姿结果；

步骤3，获得惯导的三轴加速度和三轴角速度数据、卫导数据，通过时间戳对惯导的角速度、加速度和卫导数据进行匹配，基于扩展卡尔曼滤波对惯导和卫导数据进行融合，计算得到与惯导数据同频率的惯导-卫导一级定位结果；

步骤4，利用时间戳对上述计算得出的两个一级定位结果和进行匹配，使用两个一级定位结果构建目标函数，进行全局优化，得到最终的位姿结果。

2.如权利要求1所述的一种基于惯导复用的视觉、惯导、卫导融合定位方法，其特征在于：步骤1中将惯导、双目相机、卫星天线这三种传感器刚性固连于同一载体，通过静止标定惯导零偏，使用标定板对双目相机的内参矩阵、惯导与双目相机的变换矩阵进行联合标定，测量出卫星天线到惯导的变换矩阵，卫星天线到双目相机的变换矩阵，得到惯导零偏、双目相机的内参矩阵和三种传感器之间的变换矩阵。

3.如权利要求1所述的一种基于惯导复用的视觉、惯导、卫导融合定位方法，其特征在于：步骤2.1中根据惯导的三轴加速度和三轴角速度数据计算第k帧和第k+1帧之间的积分结果，得到k+1帧的位置、速度和姿态：

（1）

（2）

（3）

式中，为世界坐标系下第k+1帧惯导的位置，为世界坐标系下第k帧惯导的位置，为世界坐标系下第k帧惯导的速度，为两个关键帧之间的时间间隔，为第t帧惯导坐标系下惯导的三轴加速度，为第t帧惯导坐标系到世界坐标系的旋转矩阵，为第t帧惯导坐标系下惯导的三轴加速度的零偏，为世界坐标系下的重力向量，为世界坐标系下第k+1帧惯导的速度，为世界坐标系下第k+1帧惯导的姿态，为世界坐标系下第k帧惯导的姿态，为四元数乘法符号，为角速度变换函数，函数是令带入函数中进行计算，为第t帧惯导坐标系下惯导的三轴角速度，为第t帧惯导坐标系下惯导的三轴角速度的零偏，为第t帧惯导坐标系相对于第k帧惯导坐标系的姿态变化量，由于惯导数据频率高于视觉数据频率，则t为第k帧视觉数据到第k+1帧视觉数据之间的惯导数据帧，世界坐标系是以第0帧惯导所在位置为原点，以重力方向、北向和东向构建的右手坐标系，惯导坐标系是以当前帧惯导所在位置为原点，以惯导硬件的陀螺、加表方向构建的右手坐标系。